package trafficmanager

import (
	"context"
	"errors"
	"fmt"
	"math"
	"strings"
	"time"

	"dev.ixmxm.top/rms/codes"
	"dev.ixmxm.top/rms/config"
	"dev.ixmxm.top/rms/managers/robotmanager"
	"dev.ixmxm.top/rms/model"
	"dev.ixmxm.top/rms/utils/angles"
	dijkstra "dev.ixmxm.top/rms/utils/dijkstra4"
	"dev.ixmxm.top/rms/utils/function"
	"dev.ixmxm.top/rms/utils/log"
	"go.mongodb.org/mongo-driver/bson"
	"go.mongodb.org/mongo-driver/mongo"
	"go.mongodb.org/mongo-driver/mongo/options"
)

func (planer *Planer) planOncePro(ctx context.Context, robot *model.Robot, mapName, source, target, realTarget string, globalRejects, globalRejectsAll, localRejects map[string]string, saveToDB, walkBack, needDelReverse bool) (*model.GlobalPath, *model.LocalPath, codes.Code, string) {
	if reachable, _ := planer.IsSiteReachable(ctx, robot, mapName, realTarget, globalRejectsAll, false); !reachable {
		return nil, nil, codes.ErrTargerNotReachable, ""
	}
	gPaths, errGPlan, gBlockAgv := planer.GlobalPathPlan(ctx, robot, source, realTarget, target, globalRejects, walkBack, saveToDB, needDelReverse)
	if errGPlan == nil {
		//规划成功，尝试局部路径规划
		lPath, errLPlan, blockAgv := planer.localPathPlanPro(ctx, robot, mapName, gPaths, localRejects, saveToDB, needDelReverse)
		if errLPlan != nil {
			// log.ErrorfWithContext(ctx, "local plan failed:%s", errLPlan.Error())
			return gPaths, nil, errLPlan, blockAgv
		}
		if len(gPaths.Paths) > 0 && len(lPath.Paths) == 0 {
			// log.ErrorfWithContext(ctx, "local plan failed:no local path")
			if realTarget != target {
				return gPaths, nil, codes.ErrLocalTransmitPathNotFound, blockAgv
			}
			return gPaths, nil, codes.ErrPathPlanNoLocalPath, blockAgv
		}
		return gPaths, lPath, nil, blockAgv
	}
	return nil, nil, errGPlan, gBlockAgv
}
func (planer *Planer) planOnce(ctx context.Context, robot *model.Robot, mapName, source, target, realTarget string, globalRejects, globalRejectsAll, localRejects map[string]string, saveToDB, walkBack, needDelReverse, avoidFlag bool, isMainTargetInGroupLocation bool) (*model.GlobalPath, *model.LocalPath, codes.Code, string) {
	if reachable, _ := planer.IsSiteReachable(ctx, robot, mapName, realTarget, globalRejectsAll, isMainTargetInGroupLocation); !reachable {
		return nil, nil, codes.ErrTargerNotReachable, ""
	}
	gPaths, errGPlan, gBlockAgv := planer.GlobalPathPlan(ctx, robot, source, realTarget, target, globalRejects, walkBack, saveToDB, needDelReverse)
	if errGPlan == nil {
		//规划成功，尝试局部路径规划
		if len(gPaths.Paths) <= 0 {
			return gPaths, &model.LocalPath{Sn: robot.Account.AgvId, MapName: mapName, Paths: model.TrafficPaths{}}, errGPlan, ""
		}
		lPath, errLPlan, blockAgv := planer.localPathPlan(ctx, robot, mapName, gPaths, localRejects, saveToDB, needDelReverse, avoidFlag)
		if errLPlan != nil {
			log.Errorf("robot:%s local plan failed:%s", robot.Account.AgvId, errLPlan.Error())
			return gPaths, nil, errLPlan, blockAgv
		}
		if len(gPaths.Paths) > 0 && len(lPath.Paths) == 0 {
			log.Errorf("robot:%s local plan failed:no local path", robot.Account.AgvId)
			if realTarget != target {
				return gPaths, nil, codes.ErrLocalTransmitPathNotFound, blockAgv
			}
			return gPaths, nil, codes.ErrPathPlanNoLocalPath, blockAgv
		}
		return gPaths, lPath, nil, blockAgv
	}
	return nil, nil, errGPlan, gBlockAgv
}
func (planer *Planer) getBestAvoidNode(ctx context.Context, robot *model.Robot, mapName, source, target string, globalRejects, globalRejectsAll, localRejects map[string]string, saveToDB, avoidFlag bool, avoid_lists map[string]model.Node, avoidPointExist bool) (*model.GlobalPath, *model.LocalPath, string, int, bool, string) {
	newTargetLayer := -1
	var blockAgv string
	if len(avoid_lists) > 0 {
		//是否启用货架访问标志
		needSendReques := planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.UseSiteStatus
		if avoidPointExist || !needSendReques {
			for _, avoid := range avoid_lists {
				if planer.IsDeadWayAreaAvoidCanNotGo(avoid.Id, target) {
					continue
				}
				//允许路线对冲区域
				gPath, lPath, errPlan, blockAgvTemp := planer.planOnce(ctx, robot, mapName, source, target, avoid.Id, globalRejects, globalRejectsAll, localRejects, saveToDB, false, true, avoidFlag, false)
				if errPlan == nil {
					return gPath, lPath, avoid.Id, newTargetLayer, true, ""
				}
				blockAgv = blockAgvTemp
			}
		} else {
			var nodes []string
			for _, avoid := range avoid_lists {
				nodes = append(nodes, avoid.Id)
			}
			goodsStatus, haveEmptyStation, err := planer.devManager.GetGoodsShelvesStauts(robot.Account.MapName, nodes)
			if (robot.Status.GoodsStatus == 0 || haveEmptyStation) && err == nil {
				for _, avoid := range avoid_lists {
					if planer.IsDeadWayAreaAvoidCanNotGo(avoid.Id, target) {
						continue
					}
					existInWMS := false
					for id, _ := range goodsStatus {
						if id == avoid.Id {
							existInWMS = true
						}
					}
					if existInWMS {
						if planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.NoIntoFullSite && goodsStatus[avoid.Id].GoodsStatus == 1 {
							continue
						}
						if robot.Status.GoodsStatus == 1 && goodsStatus[avoid.Id].GoodsStatus == 1 {
							continue
						}
						newTargetLayer = goodsStatus[avoid.Id].Layer
						log.Infof("newTargetLayer = %d", newTargetLayer)
					}

					gPath, lPath, errPlan, blockAgvTemp := planer.planOnce(ctx, robot, mapName, source, target, avoid.Id, globalRejects, globalRejectsAll, localRejects, saveToDB, false, true, avoidFlag, false)
					if errPlan == nil {
						return gPath, lPath, avoid.Id, newTargetLayer, true, ""
					}
					blockAgv = blockAgvTemp
				}
			}
		}
	}
	return nil, nil, "0", newTargetLayer, false, blockAgv
}
func (planer *Planer) PathPlanNew(ctx context.Context, robot *model.Robot, subtask *model.Subtask, globalRejects, globalRejectsAll, localRejects map[string]string, avoidFlag, saveToDB, retry bool) (*model.GlobalPath, *model.LocalPath, model.TaskSubStatus, string, int, codes.Code, string, *model.GlobalPath) {
	inRobotCoverNodes := func(id string, coverNodes []string) bool {
		for _, n := range coverNodes {
			if n == id {
				return true
			}
		}
		return false
	}
	inAreaRejectIds := func(id string) bool {
		for _, area := range planer.m.MapData.Areas {
			if (area.Type == model.AreaTypeAllowPathMutex || area.Type == model.AreaTypeDeadWay) && len(area.ContainDotIds) > 0 {
				for _, d := range area.ContainDotIds {
					if d == id {
						return true
					}
				}
			}
		}
		return false
	}
	var gPath, gPathTmp *model.GlobalPath
	var lPath, lPathTmp *model.LocalPath
	var errPlan codes.Code
	var blockAgv, blockAgvTemp string
	source := robot.Status.SiteIdS
	if source == "0" && robot.Status.Status != model.RobotStatusIdle {
		source = robot.LastSiteId
	}
	targetNode := planer.GetNode(subtask.TargetSiteID)
	rmsTaskTargetNode := planer.GetNode(subtask.MainTaskTargetSiteID)
	//判断最终任务目标点是否在地堆库中
	findMainTaskGroupLocation := planer.FindRelationGroupLocatinByNodeId(rmsTaskTargetNode.Id)
	IsMainTaskInGroupLocation := false
	if findMainTaskGroupLocation != nil {
		IsMainTaskInGroupLocation = true
	}
	log.Warnf("robot:%s source:%s target:%s globalRejects:%+v", robot.Account.AgvId, source, subtask.TargetSiteID, globalRejects)
	gPurePaths, global_ids := planer.getGlobalPath(ctx, robot, source, subtask.TargetSiteID, globalRejects)
	//目标点全局路径求解
	gPath, lPath, errPlan, blockAgv = planer.planOnce(ctx, robot, subtask.MapName, source, subtask.TargetSiteID, subtask.TargetSiteID, globalRejects, globalRejectsAll, localRejects, saveToDB, false, true, avoidFlag, IsMainTaskInGroupLocation)
	if errPlan == nil {
		return gPath, lPath, model.WalkToTarget, subtask.TargetSiteID, -1, nil, blockAgv, gPurePaths
	} else if errPlan == codes.ErrCallDevice {
		return gPath, lPath, model.InitState, "0", -1, errPlan, blockAgv, gPurePaths
	}
	if errPlan != nil {
		log.Errorf("错误：%+v", errPlan)
	}
	if retry && blockAgv == "" && gPurePaths != nil {
		find := false
		targetSiteId := ""
		for i, p := range gPurePaths.Paths {
			for _, r := range robotmanager.GetAllRobotsInSameMapFromCache(robot.Account.MapName) {
				if r.Account.AgvId == robot.Account.AgvId {
					continue
				}
				if !r.StatusIsExists() {
					continue
				}

				polygonConflict := false
				robotMoving := false
				gotReverseAgv := false
				oldLocalPath, _ := planer.getRobotLocalPaths(context.Background(), r.Account.AgvId)
				if oldLocalPath != nil && oldLocalPath.Paths != nil {
					robotMoving = true
				}
				if !robotMoving {
					if inRobotCoverNodes(p.GetTarget().Id, r.AgvCorverNodes) {
						polygonConflict = true
					} else {
						if oldLocalPath != nil && oldLocalPath.CurrentNode.Id != "0" && oldLocalPath.CurrentNode.Id != "" {
							if oldLocalPath.NodeRect.ConflictPolygon(p.PathPolygon) {
								polygonConflict = true
							}
						}
					}
				} else {
					if conflict, _ := oldLocalPath.Conflict(p, planer.getConnectPaths, false, robot); conflict {
						if gPaths, _ := planer.GetRobotGlobalPaths(context.Background(), r.Account.AgvId); gPaths != nil && gPaths.Paths != nil {
							if blockPathExist(p.PathId, gPaths.Paths.ToIDs()) {
								polygonConflict = true
								gotReverseAgv = true
							}
						}
						find = true
						break
					}
				}
				if polygonConflict {
					find = true
					blockAgv = r.Account.AgvId
					gPurePaths.BlockStart = p.PathId
					angleRad := angles.Deg2Rad(float64(p.PathEndAng) - float64(r.Status.Angle))
					if i > 3 && (math.Abs(angles.NormalizeAngle(angleRad)) < math.Pi/18 || gotReverseAgv) {
						for j := 1; i-j > 1; j++ {
							if !inAreaRejectIds(gPurePaths.Paths[i-j].GetTarget().Id) {
								targetSiteId = gPurePaths.Paths[i-j].GetTarget().Id
								break
							}
						}
					}
					break
				}
			}
			if find {
				break
			}
		}
		if find && errPlan == codes.ErrPathPlanNoGlobalPath {
			if targetSiteId != "" {
				gPath, lPath, errPlan, blockAgvTemp = planer.planOnce(ctx, robot, subtask.MapName, source, subtask.TargetSiteID, targetSiteId, globalRejects, globalRejectsAll, localRejects, saveToDB, false, true, avoidFlag, false)
				if errPlan == nil {
					return gPath, lPath, model.WalkToTarget, targetSiteId, -1, nil, blockAgvTemp, gPurePaths
				}
			}
			return gPurePaths, lPath, model.InitState, "0", -1, codes.ErrPathPlanNoGlobalPath, blockAgv, gPurePaths
		}
	}

	//存在中转点,且机器人不在中转点
	log.Warnf("avoidFlag:%t global_ids lens:%d  targetNode.IntermediateNode:%s globalRejects:%v ", avoidFlag, len(global_ids), targetNode.IntermediateNode, globalRejects)
	// if targetNode.IntermediateNode != "" && (targetNode.LocationType != model.LocationGround) {
	if targetNode.IntermediateNode != "" {
		//存在中转点,且机器人在中转点
		if source == targetNode.IntermediateNode {
			log.InfofWithContext(ctx, "机器人正在中转点")
			return nil, nil, model.InitState, "0", -1, codes.ErrAlreadyInTransmitPoint, blockAgv, gPurePaths
		}
		if avoidFlag && source != targetNode.IntermediateNode {
			gPathTmp, lPathTmp, errPlan, blockAgvTemp = planer.planOnce(ctx, robot, subtask.MapName, source, subtask.TargetSiteID, targetNode.IntermediateNode, globalRejects, globalRejectsAll, localRejects, saveToDB, false, true, avoidFlag, false)
			if blockAgvTemp != "" {
				blockAgv = blockAgvTemp
				gPath = gPathTmp
			}
			if errPlan == nil {
				gPath = gPathTmp
				lPath = lPathTmp
				return gPath, lPath, model.WalkToTemp, targetNode.IntermediateNode, -1, nil, blockAgv, gPurePaths
			}
			_, global_tmp_ids := planer.getGlobalPath(ctx, robot, source, targetNode.IntermediateNode, globalRejects)
			if global_tmp_ids != nil {
				avoid_lists, avoidPointExist, staySame := planer.getGlobalPathAvoidList(ctx, robot, source, targetNode.IntermediateNode, global_tmp_ids, globalRejectsAll)
				if staySame {
					return nil, nil, model.InitState, "0", -1, codes.ErrGlobalTransmitPathNotFound, blockAgv, gPurePaths
				}
				newTargetLayer := -1
				avoidId := "0"
				getBest := false
				gPathTmp, lPathTmp, avoidId, newTargetLayer, getBest, blockAgvTemp = planer.getBestAvoidNode(ctx, robot, subtask.MapName, source, subtask.TargetSiteID, globalRejects, globalRejectsAll, localRejects, saveToDB, avoidFlag, avoid_lists, avoidPointExist)
				if blockAgvTemp != "" {
					blockAgv = blockAgvTemp
					gPath = gPathTmp
				}
				if getBest {
					gPath = gPathTmp
					lPath = lPathTmp
					return gPath, lPath, model.WalkToTempReplace, avoidId, newTargetLayer, nil, blockAgv, gPurePaths
				}
			}
		}

	} else if global_ids != nil && len(global_ids) > 0 {
		avoid_lists, avoidPointExist, staySame := make(map[string]model.Node, 0), false, false
		if robot.Account.MapName == "1-1@A1" {
			avoid_lists, avoidPointExist, staySame = planer.getGlobalPathAvoidList2(ctx, robot, source, subtask.TargetSiteID, global_ids, globalRejectsAll)
		} else {
			if avoidFlag {
				avoid_lists, avoidPointExist, staySame = planer.getGlobalPathAvoidList1(ctx, robot, source, subtask.TargetSiteID, global_ids, globalRejectsAll)
			} else {
				avoid_lists, avoidPointExist, staySame = planer.getGlobalPathAvoidList2(ctx, robot, source, subtask.TargetSiteID, global_ids, globalRejectsAll)
			}
		}
		if staySame {
			return nil, nil, model.InitState, "0", -1, codes.ErrPathPlanNoGlobalPath, blockAgv, gPurePaths
		}
		newTargetLayer := -1
		avoidId := "0"
		getBest := false

		gPathTmp, lPathTmp, avoidId, newTargetLayer, getBest, blockAgvTemp = planer.getBestAvoidNode(ctx, robot, subtask.MapName, source, subtask.TargetSiteID, globalRejects, globalRejectsAll, localRejects, saveToDB, avoidFlag, avoid_lists, avoidPointExist)
		log.Warnf("robot:%s current bestAvoidId:%s", robot.Account.AgvId, avoidId)
		if blockAgvTemp != "" {
			blockAgv = blockAgvTemp
			gPath = gPathTmp
		}
		if getBest {
			gPath = gPathTmp
			lPath = lPathTmp
			return gPath, lPath, model.WalkToTargetReplace, avoidId, newTargetLayer, nil, blockAgv, gPurePaths
		}
		return gPath, lPath, model.InitState, "0", -1, codes.ErrPathPlanNoGlobalPath, blockAgv, gPurePaths

	}
	return gPath, lPath, model.InitState, "0", -1, codes.ErrPathPlanNoGlobalPath, blockAgv, gPurePaths
}
func (planer *Planer) PathPlanBlock(ctx context.Context, robot *model.Robot, subtask *model.Subtask, globalRejects, globalRejectsAll, localRejects map[string]string) (*model.GlobalPath, *model.LocalPath, model.TaskSubStatus, string, int, codes.Code, string) {
	arrCompare := func(src, des []string) bool {
		if len(src) == 0 || len(des) == 0 || len(src) != len(des) {
			return false
		}
		for index, srcValue := range src {
			if srcValue != des[index] {
				return false
			}
		}
		return true
	}

	var blockAgv string
	systemCfg, _ := robotmanager.GetConfigFromDb(context.Background())
	if time.Now().Sub(subtask.BlockAvoidTime) < time.Duration(int64(planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.WaitTime))*time.Second {
		return nil, nil, model.InitState, "0", -1, codes.ErrNoBlockPath, subtask.BlockAgv
	}
	if subtask.BlockAgv != "" {
		blockRobot := robotmanager.GetFromCache(subtask.BlockAgv)
		if blockRobot.Status != nil {
			if subtask.BlockAgvSite == blockRobot.Status.SiteIdS && subtask.SubStatus != model.WalkToTarget &&
				time.Now().Sub(subtask.BlockAvoidTime) < time.Duration(systemCfg.TrafficCfg.TaskAvoidCfg.AvoidTriggerTime)*time.Second {
				//log.Warnf("子任务:%d, blockRobot=%s未挪动，不进行重规划", subtask.Id, blockRobot.Account.AgvId)
				return nil, nil, model.InitState, "0", -1, codes.ErrNoBlockPath, subtask.BlockAgv
			}
		}
	}
	//堵塞首先计算前端避让点
	// if planer.GetNode(robot.Status.SiteIdS).Type == model.NodePath {
	// 	frontTarget, frontPaths := planer.getAgvFrontAvoidNode(ctx, robot, localRejects)
	// 	if frontTarget != robot.Status.SiteIdS {
	// 		gPath, lPath, errPlan, blockAgvTemp := planer.planOnce(ctx, robot, subtask.MapName, robot.Status.SiteIdS, subtask.TargetSiteID, frontTarget, globalRejects, globalRejectsAll, localRejects, true, false, true, true)
	// 		if errPlan == nil {
	// 			gPathIds := gPath.Paths.ToIDs()
	// 			//比较规划路径是否完全相同
	// 			if arrCompare(frontPaths, gPathIds) {
	// 				log.Warnf("frontTarget = %s", frontTarget)
	// 				return gPath, lPath, model.WalkForwardAvoid, frontTarget, -1, nil, ""
	// 			}
	// 		}
	// 		if blockAgvTemp != "" {
	// 			blockAgv = blockAgvTemp
	// 		}
	// 	}
	// }
	//堵塞然后计算就近避让点
	avoid_lists, avoidPointExist := planer.getAgvNearAvoidList(ctx, robot, globalRejectsAll)
	gPath, lPath, avoidId, newTargetLayer, getBest, blockAgvTemp := planer.getBestAvoidNode(ctx, robot, subtask.MapName, robot.Status.SiteIdS, subtask.TargetSiteID, globalRejects, globalRejectsAll, localRejects, true, true, avoid_lists, avoidPointExist)
	if getBest {
		log.Warnf("nearTarget = %s", avoidId)
		return gPath, lPath, model.WalkNearAvoid, avoidId, newTargetLayer, nil, ""
	}
	if blockAgvTemp != "" {
		blockAgv = blockAgvTemp
	}
	//最后计算后退避让点
	backTarget, backPaths := planer.getAgvBackAvoidNode(ctx, robot, localRejects)
	if backTarget != "" && backTarget != "0" && backTarget != robot.Status.SiteIdS {
		gPath, lPath, errPlan, _ := planer.planOnce(ctx, robot, subtask.MapName, robot.Status.SiteIdS, subtask.TargetSiteID, backTarget, globalRejects, globalRejectsAll, localRejects, true, true, true, false, false)
		if errPlan == nil {
			gPathIds := gPath.Paths.ToIDs()
			//比较规划路径是否完全相同
			if arrCompare(backPaths, gPathIds) {
				log.Warnf("backTarget = %s", backTarget)
				return gPath, lPath, model.WalkBackforwardAvoid, backTarget, -1, nil, ""
			}
		}
	}
	return nil, nil, model.InitState, "0", -1, codes.ErrNoBlockPath, blockAgv
}

//GlobalPathPlan 全局路径规划
func (planer *Planer) GlobalPathPlan(ctx context.Context, robot *model.Robot, source, target, finalTarget string, rejectsAll map[string]string, walkBack, saveToDB, needDelReverse bool) (*model.GlobalPath, codes.Code, string) {
	if source == target {
		return &model.GlobalPath{Sn: robot.Account.AgvId}, nil, ""
	}
	bestGPath, errPlan, blockAgv := planer.getBestGlobalPath(ctx, robot, source, target, finalTarget, rejectsAll, walkBack, needDelReverse)
	if errPlan != nil {
		return nil, codes.ErrPathPlanNoGlobalPath, blockAgv
	}
	if saveToDB {
		if err := planer.saveGlobalPathToDB(ctx, bestGPath); err != nil {
			log.ErrorfWithContext(ctx, "add robot:%s global path to db error:%s", robot.Account.AgvId, err.Error())
			return nil, codes.ErrDatabase.Wrap(err), ""
		}
	}
	//log.Infof("全局路径:代价值%f,路径:%s", bestGPath.Cost, bestGPath.Paths.ToString())
	return bestGPath, nil, ""
}

//DeleteRobotGlobalPaths 清除机器人全局路径
func (planer *Planer) DeleteRobotGlobalPaths(ctx context.Context, sn string, mapName string) codes.Code {
	log.Infof("robot:%s 开始清除机器人全局路径", sn)
	planer.pathLock.Lock()
	delete(planer.agvNeedPaths, sn)
	planer.pathLock.Unlock()
	_, err := planer.db.Collection(ctx, model.GlobalPath{}.TableName()).DeleteOne(ctx, bson.M{"sn": sn, "map_name": mapName})
	if err != nil && !errors.Is(err, mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "delete robot:%s global path error:%s", sn, err.Error())
		return codes.ErrDatabase.Wrap(err)
	}
	return nil
}

func (planer *Planer) saveGlobalPathToDB(ctx context.Context, globalPath *model.GlobalPath) codes.Code {
	if globalPath.Sn == "" {
		log.Panicf("saveGlobalPathToDB empty sn")
	}
	dbRes := planer.db.Collection(ctx, model.GlobalPath{}.TableName()).FindOneAndReplace(ctx, bson.M{"sn": globalPath.Sn}, globalPath, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "save local path to db error:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	return nil
}

//点位是否可达
func (planer *Planer) IsTaskTargetIsReachable(ctx context.Context, currentRobot *model.Robot, mapName, siteId string, rejectsAll map[string]string, isMainTargetInGroupLocation bool) (bool, codes.Code) {
	robotId := currentRobot.Account.AgvId
	node, find := planer.m.MapData.NodesMap[siteId]
	if !find {
		log.Errorf("robot:%s 目标点：%s 不可达，mapName:%s ,因为目标点地图节点不存在！！！", robotId, siteId, mapName)
		return false, codes.ErrMapNodeNotFound
	}

	for locksite, _ := range rejectsAll {
		if siteId == locksite {
			log.Errorf("robot:%s 目标点：%s 不可达，mapName:%s ,因为目标点属于剔除点！！！", robotId, siteId, mapName)
			return false, nil
		}
	}

	//点位是否在互斥区域、小车限数区、交互区域
	rejectIds, err2 := planer.GetPlanerAreaRejectNodes(ctx, currentRobot, nil)
	if err2 != nil {
		return false, nil
	}
	if _, find := rejectIds[siteId]; find {
		log.Warnf("robot:%s 目标点：%s %s 不可达,目标点被踢除(互斥区域or限数区or第三方交互区)！！！", robotId, siteId, mapName)
		return false, nil
	}
	robots := robotmanager.GetAllRobotsInSameMapFromCache(currentRobot.Account.MapName)
	for _, robot := range robots {
		if robotId == robot.Account.AgvId || robot.Status == nil || !robot.Status.Online || !robot.Status.TaskOnline {
			continue
		}
		//点位是否有车
		if robot.Status.MapName == mapName && robot.Status.SiteIdS == siteId && node.Type != model.NodePath {
			log.Errorf("robot:%s 目标点：%s %s不可达,目标点被AGV:%s占用！！！", robotId, siteId, mapName, robot.Account.AgvId)
			return false, nil
		}
		//绑定点是否有车
		if len(node.BanNodes) > 0 {
			for _, id := range node.BanNodes {
				if robot.Status.MapName == mapName && robot.Status.SiteIdS == id {
					log.Errorf("robot:%s 目标点：%s %s 不可达，目标点的绑定点，被AGV:%s占用！！！", robotId, siteId, mapName, robot.Account.AgvId)
					return false, nil
				}
			}
		}
	}
	return true, nil
}

//点位是否可达
func (planer *Planer) IsSiteReachable(ctx context.Context, currentRobot *model.Robot, mapName, siteId string, rejectsAll map[string]string, isMainTargetInGroupLocation bool) (bool, codes.Code) {
	robotId := currentRobot.Account.AgvId
	node, find := planer.m.MapData.NodesMap[siteId]
	if !find {
		log.Errorf("robot:%s 目标点：%s 不可达，mapName:%s ,因为目标点地图节点不存在！！！", robotId, siteId, mapName)
		return false, codes.ErrMapNodeNotFound
	}

	for locksite, _ := range rejectsAll {
		if siteId == locksite {
			log.Errorf("robot:%s 目标点：%s 不可达，mapName:%s ,因为目标点属于剔除点！！！", robotId, siteId, mapName)
			return false, nil
		}
	}

	//点位是否在互斥区域、小车限数区、交互区域
	// rejectIds, err2 := planer.GetPlanerAreaRejectNodes(ctx, currentRobot, nil)
	// if err2 != nil {
	// 	return false, nil
	// }
	// if _, find := rejectIds[siteId]; find {
	// 	log.Warnf("robot:%s 目标点：%s %s 不可达,目标点被踢除(互斥区域or限数区or第三方交互区)！！！", robotId, siteId, mapName)
	// 	return false, nil
	// }
	robots := robotmanager.GetAllRobotsInSameMapFromCache(currentRobot.Account.MapName)
	for _, robot := range robots {
		if robotId == robot.Account.AgvId || robot.Status == nil || !robot.Status.Online || !robot.Status.TaskOnline {
			continue
		}
		//点位是否有车
		if robot.Status.MapName == mapName && robot.Status.SiteIdS == siteId && node.Type != model.NodePath {
			log.Errorf("robot:%s 目标点：%s %s不可达,目标点被AGV:%s占用！！！", robotId, siteId, mapName, robot.Account.AgvId)
			return false, nil
		}
		//绑定点是否有车
		if len(node.BanNodes) > 0 {
			for _, id := range node.BanNodes {
				if robot.Status.MapName == mapName && robot.Status.SiteIdS == id {
					log.Errorf("robot:%s 目标点：%s %s 不可达，目标点的绑定点，被AGV:%s占用！！！", robotId, siteId, mapName, robot.Account.AgvId)
					return false, nil
				}
			}
		}
	}
	return true, nil
}
func (planer *Planer) getGlobalPathAvoidList(ctx context.Context, robot *model.Robot, source, target string, global_ids []string, globalRejectsAll map[string]string) (map[string]model.Node, bool, bool) {
	existInList := func(n model.Node, list []model.Node) bool {
		for _, tmp := range list {
			if tmp.Id == n.Id {
				return true
			}
		}
		return false
	}
	existInPathList := func(n string, list []string) bool {
		for _, tmp := range list {
			if tmp == n {
				return true
			}
		}
		return false
	}
	avoid_lists := make(map[string]model.Node, 0)
	olds := make([]string, 0)
	// targetNode := planer.m.MapData.NodesMap[target]
	for k := len(global_ids) - 2; k > 0; k-- {
		allNodes := make(map[string]model.Node, 0)
		global_id := global_ids[k]
		node := planer.m.MapData.NodesMap[global_id]

		lastNodes := make([]model.Node, 0)
		lastNodes = append(lastNodes, node)
		for i := 0; i < config.Conf.GlobalExtendNum; i++ {
			newArr := make([]model.Node, 0)
			for j := 0; j <= len(lastNodes)-1; j++ {
				nodeConnectPaths := planer.GetConnectPathsInStart(lastNodes[j])
				if !existInList(lastNodes[j], newArr) {
					newArr = append(newArr, lastNodes[j])
				}
				for _, nodePath := range nodeConnectPaths {
					node1 := nodePath.GetAnother(lastNodes[j])
					if existInPathList(node1.Id, global_ids) {
						continue
					}
					if !existInList(node1, newArr) && !existInPathList(node1.Id, olds) {
						if node1.AllowAvoid == 1 && (node1.Type == model.NodeStation || node1.Type == model.NodeAvoid || node1.Type == model.NodeWaiting || node1.Type == model.NodeCharge) {
							allNodes[node1.Id] = node1
						}
						newArr = append(newArr, node1)
					}
				}
			}
			if len(newArr) == len(lastNodes) {
				break
			}
			lastNodes = make([]model.Node, len(newArr))
			//将数组拷贝
			copy(lastNodes, newArr)
		}

		if _, find := allNodes[source]; find {
			return nil, true, true
		}
		for _, tmpNode := range allNodes {
			olds = append(olds, tmpNode.Id)
			if tmpNode.Id == target {
				continue
			}
			//如果判断点的类别为避让点，直接返回该点
			if tmpNode.Type == model.NodeAvoid || tmpNode.Type == model.NodeCharge || tmpNode.Type == model.NodeWaiting {
				goalReachable, _ := planer.IsSiteReachable(ctx, robot, robot.Account.MapName, tmpNode.Id, globalRejectsAll, false)
				if !goalReachable {
					continue
				}
				for k := range avoid_lists {
					delete(avoid_lists, k)
				}
				avoid_lists[tmpNode.Id] = tmpNode
				return avoid_lists, true, false
			}
			if _, find := avoid_lists[tmpNode.Id]; !find {
				goalReachable, _ := planer.IsSiteReachable(ctx, robot, robot.Account.MapName, tmpNode.Id, globalRejectsAll, false)
				if !goalReachable {
					continue
				}
				avoid_lists[tmpNode.Id] = tmpNode
			}
			if len(avoid_lists) >= planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.CheckAvoidSiteNumber {
				break
			}
		}
		if len(avoid_lists) >= planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.CheckAvoidSiteNumber {
			break
		}
	}
	return avoid_lists, false, false
}

//749避让点选择逻辑
func (planer *Planer) getGlobalPathAvoidList1(ctx context.Context, robot *model.Robot, source, target string, global_ids []string, globalRejectsAll map[string]string) (map[string]model.Node, bool, bool) {
	existInList := func(n model.Node, list []model.Node) bool {
		for _, tmp := range list {
			if tmp.Id == n.Id {
				return true
			}
		}
		return false
	}
	existInPathList := func(n string, list []string) bool {
		for _, tmp := range list {
			if tmp == n {
				return true
			}
		}
		return false
	}
	//
	avoidList, flag1, flag2 := planer.getGlobalPathAvoidList2(ctx, robot, source, target, global_ids, globalRejectsAll)
	if len(avoidList) > 0 {
		return avoidList, flag1, flag2
	}
	avoid_lists := make(map[string]model.Node, 0)
	olds := make([]string, 0)
	// targetNode := planer.m.MapData.NodesMap[target]
	for k := len(global_ids) - 2; k > 0; k-- {

		allNodes := make(map[string]model.Node, 0)
		global_id := global_ids[k]
		node := planer.m.MapData.NodesMap[global_id]

		lastNodes := make([]model.Node, 0)
		lastNodes = append(lastNodes, node)

		for i := 0; i < config.Conf.GlobalExtendNum; i++ {
			newArr := make([]model.Node, 0)
			for j := 0; j <= len(lastNodes)-1; j++ {
				nodeConnectPaths := planer.GetConnectPathsInStart(lastNodes[j])
				if !existInList(lastNodes[j], newArr) {
					newArr = append(newArr, lastNodes[j])
				}
				for _, nodePath := range nodeConnectPaths {
					node1 := nodePath.GetAnother(lastNodes[j])
					if existInPathList(node1.Id, global_ids) {
						continue
					}
					if !existInList(node1, newArr) && !existInPathList(node1.Id, olds) {
						if node1.AllowAvoid == 1 && (node1.Type == model.NodeStation || node1.Type == model.NodeAvoid || node1.Type == model.NodeWaiting || node1.Type == model.NodeCharge) {
							//目标点和避让点不能在同一个死胡同
							if planer.IsDeadWayAreaAvoidCanNotGo(node1.Id, target) {
								continue
							}
							allNodes[node1.Id] = node1
						}
						newArr = append(newArr, node1)
					}
				}
			}
			if len(newArr) == len(lastNodes) {
				break
			}
			lastNodes = make([]model.Node, len(newArr))
			//将数组拷贝
			copy(lastNodes, newArr)
		}

		if _, find := allNodes[source]; find {
			return nil, true, true
		}

		for _, tmpNode := range allNodes {
			olds = append(olds, tmpNode.Id)
			if tmpNode.Id == target {
				continue
			}
			//如果判断点的类别为避让点，直接返回该点
			if tmpNode.Type == model.NodeAvoid || tmpNode.Type == model.NodeCharge || tmpNode.Type == model.NodeWaiting {
				goalReachable, _ := planer.IsSiteReachable(ctx, robot, robot.Account.MapName, tmpNode.Id, globalRejectsAll, false)
				if !goalReachable {
					continue
				}
				for k := range avoid_lists {
					delete(avoid_lists, k)
				}
				avoid_lists[tmpNode.Id] = tmpNode
				return avoid_lists, true, false
			}
			if _, find := avoid_lists[tmpNode.Id]; !find {
				goalReachable, _ := planer.IsSiteReachable(ctx, robot, robot.Account.MapName, tmpNode.Id, globalRejectsAll, false)
				if !goalReachable {
					continue
				}
				avoid_lists[tmpNode.Id] = tmpNode
			}
			if len(avoid_lists) >= planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.CheckAvoidSiteNumber {
				break
			}
		}
		if len(avoid_lists) >= planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.CheckAvoidSiteNumber {
			break
		}
	}
	return avoid_lists, false, false
}

//749避让点选择逻辑
func (planer *Planer) getGlobalPathAvoidList2(ctx context.Context, robot *model.Robot, source, target string, global_ids []string, globalRejectsAll map[string]string) (map[string]model.Node, bool, bool) {

	avoid_lists := make(map[string]model.Node, 0)
	// targetNode := planer.m.MapData.NodesMap[target]
	for k := len(global_ids) - 2; k > 0; k-- {

		global_id := global_ids[k]
		node := planer.m.MapData.NodesMap[global_id]
		if global_id == source {
			return nil, true, true
		}

		//如果判断点的类别为避让点，直接返回该点
		if node.Type == model.NodeAvoid || node.Type == model.NodeCharge || node.Type == model.NodeWaiting {
			//目标点和避让点不能在同一个死胡同
			if planer.IsDeadWayAreaAvoidCanNotGo(node.Id, target) {
				continue
			}
			goalReachable, _ := planer.IsSiteReachable(ctx, robot, robot.Account.MapName, node.Id, globalRejectsAll, false)
			if !goalReachable {
				continue
			}
			for k := range avoid_lists {
				delete(avoid_lists, k)
			}
			avoid_lists[node.Id] = node
			return avoid_lists, true, false
		}
	}

	return avoid_lists, false, false
}
func (planer *Planer) getAgvNearAvoidList(ctx context.Context, robot *model.Robot, globalRejectsAll map[string]string) (map[string]model.Node, bool) {
	avoid_lists := make(map[string]model.Node, 0)
	current_id := robot.Status.SiteIdS
	node := planer.m.MapData.NodesMap[current_id]
	allNodes := make([]model.Node, 0)

	rejectArea := planer.GetAreaContainIdsByAreaTypes([]model.AreaType{model.AreaTypeAllowPathMutex, model.AreaTypeNoParkingArea}...)
	for key, value := range rejectArea {
		globalRejectsAll[key] = value
	}
	existInList := func(n model.Node, list []model.Node) bool {
		for _, tmp := range list {
			if tmp.Id == n.Id {
				return true
			}
		}
		return false
	}
	getConnectNodeOnce := func(node model.Node) []model.Node {
		allOnceNodes := make([]model.Node, 0)
		nodeConnectPaths := planer.GetConnectPathsInStart(node)
		allOnceNodes = append(allOnceNodes, node)
		for _, nodePath := range nodeConnectPaths {
			nodeAnother := nodePath.GetAnother(node)
			if !existInList(nodeAnother, allOnceNodes) {
				allOnceNodes = append(allOnceNodes, nodeAnother)
			}
		}
		return allOnceNodes
	}
	allNodes = getConnectNodeOnce(node)
	sum := planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.NearCalculateNumber
	allOlds := make(map[string]string, 0)
	for i := 0; i < sum; i++ {
		allNodesTmp := make([]model.Node, 0)
		for _, tmpNode := range allNodes {
			for _, tmp0 := range getConnectNodeOnce(tmpNode) {
				if !existInList(tmp0, allNodesTmp) {
					allNodesTmp = append(allNodesTmp, tmp0)
				}
			}
		}
		allNodes = allNodesTmp
		for _, tmpNode := range allNodes {
			if tmpNode.Id == current_id {
				continue
			}
			if _, find := allOlds[tmpNode.Id]; find {
				continue
			}
			allOlds[tmpNode.Id] = tmpNode.Id
			//如果判断点的类别为避让点，直接返回该点
			if tmpNode.Type == model.NodeAvoid || ((tmpNode.Type == model.NodeCharge || tmpNode.Type == model.NodeWaiting) && tmpNode.AllowAvoid == 1) {
				goalReachable, _ := planer.IsSiteReachable(ctx, robot, robot.Account.MapName, tmpNode.Id, globalRejectsAll, false)
				if !goalReachable {
					continue
				}
				for k := range avoid_lists {
					delete(avoid_lists, k)
				}
				avoid_lists[tmpNode.Id] = tmpNode
				return avoid_lists, true
			}
			//todo 如果点位不可达，也是continue
			if tmpNode.AllowAvoid == 1 && (tmpNode.Type == model.NodeStation || tmpNode.Type == model.NodeAvoid || tmpNode.Type == model.NodeWaiting || tmpNode.Type == model.NodeCharge) {
				goalReachable, _ := planer.IsSiteReachable(ctx, robot, robot.Account.MapName, tmpNode.Id, globalRejectsAll, false)
				if !goalReachable {
					continue
				}
				if _, find := avoid_lists[tmpNode.Id]; !find {
					avoid_lists[tmpNode.Id] = tmpNode
				}
			}
			if len(avoid_lists) >= planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.CheckAvoidSiteNumber {
				break
			}
		}
		if len(avoid_lists) >= planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.CheckAvoidSiteNumber {
			break
		}
	}
	return avoid_lists, false
}

//为空闲车辆寻找避让点
func (planer *Planer) getIdleAgvNearAvoidList(ctx context.Context, robot *model.Robot, robots []*model.Robot, agvSite string, globalRejects, globalRejectsAll, localRejects, goalRejects map[string]string, freeAvoidRejects map[string]string, gPaths model.TrafficPaths) model.Node {
	currentRobotStatus := robot.Status
	current_id := currentRobotStatus.SiteIdS
	//inAreaFlag := planer.GetCurrentSiteIsInArea(robot.Status.SiteIdS, model.AreaTypeDeadWay)
	node := planer.m.MapData.NodesMap[current_id]
	allNodes := make(map[string]model.Node, 0)
	pathBlockCircle := func(p1, p2 model.Pose2, x, y, radius float64) bool {
		xa, ya, xb, yb := p1.X, p1.Y, p2.X, p2.Y
		line_dis := 99999.0
		if (xb-xa)*(x-xa)+(yb-ya)*(y-ya) < 0 {
			line_dis = math.Sqrt((xa-x)*(xa-x) + (ya-y)*(ya-y))
		} else if (xa-xb)*(x-xb)+(ya-yb)*(y-yb) < 0 {
			line_dis = math.Sqrt((xb-x)*(xb-x) + (yb-y)*(yb-y))
		} else {
			if xa == xb {
				line_dis = math.Abs(x - xa)
			} else if ya == yb {
				line_dis = math.Abs(y - ya)
			} else {
				k := (yb - ya) / (xb - xa)
				b := yb - k*xb
				line_dis = math.Abs(k*x-y+b) / math.Sqrt(k*k+1)
			}
		}
		if line_dis < radius {
			return true
		}
		return false
	}
	RotateConflictRect := func(x, y, radius float64) (bool, string) {
		allApprovedLocalPaths, _ := planer.GetAllLocalPathFromDB(context.Background()) //(ctx)
		for _, lp := range allApprovedLocalPaths {
			if lp.Sn == robot.Account.AgvId {
				continue
			}
			if lp.Paths != nil {
				for k := len(lp.Paths) - 1; k >= 0; k-- {
					p := lp.Paths[k]
					for i := 0; i <= len(p.PathPolygon)-1; i++ {
						if i == len(p.PathPolygon)-1 {
							if pathBlockCircle(p.PathPolygon[len(p.PathPolygon)-1], p.PathPolygon[0], x, y, radius) {
								return true, lp.Sn
							}
						} else {
							if pathBlockCircle(p.PathPolygon[i], p.PathPolygon[i+1], x, y, radius) {
								return true, lp.Sn
							}
						}
					}
					if p.RotateType != 0 {
						dis := math.Hypot(x-float64(p.GetSource().X), y-float64(p.GetSource().Y))
						if dis < 2*radius {
							return true, lp.Sn
						}
					}
					if p.TargetRotate != 0 {
						dis := math.Hypot(x-float64(p.GetTarget().X), y-float64(p.GetTarget().Y))
						if dis < 2*radius {
							return true, lp.Sn
						}
					}
				}
			} else {
				for i := 0; i <= len(lp.NodeRect)-1; i++ {
					if i == len(lp.NodeRect)-1 {
						if pathBlockCircle(lp.NodeRect[len(lp.NodeRect)-1], lp.NodeRect[0], x, y, radius) {
							return true, lp.Sn
						}
					} else {
						if pathBlockCircle(lp.NodeRect[i], lp.NodeRect[i+1], x, y, radius) {
							return true, lp.Sn
						}
					}
				}
			}
		}
		return false, ""
	}
	pathEndBlockGlobalPath := func(node model.Node, robotAngle float64, paths model.TrafficPaths) bool {
		rect := robot.Type.GotLockRectangle(robot, robotAngle, float64(node.X), float64(node.Y), 0, node.Type, false, false)
		for _, item := range paths {
			if item.PathPolygon.ConflictPolygon(rect) {
				return true
			}
		}
		return false
	}
	getConnectNodeOnce := func(node model.Node, olds map[string]model.Node) map[string]model.Node {
		allOnceNodes := make(map[string]model.Node, 0)
		nodeConnectPaths := planer.GetConnectPathsInStart(node)
		allOnceNodes[node.Id] = node
		for _, nodePath := range nodeConnectPaths {
			nodeAnother := nodePath.GetAnother(node)
			if agvSite == nodeAnother.Id {
				continue
			}
			if _, find := localRejects[nodeAnother.Id]; find {
				continue
			}
			if _, find := olds[nodeAnother.Id]; find {
				continue
			}
			if nodeAnother.Type == 0 || nodeAnother.Type == model.NodePath || nodeAnother.Type == model.NodeAvoid ||
				((nodeAnother.Type == model.NodeWaiting || nodeAnother.Type == model.NodeCharge) && nodeAnother.AllowAvoid == 1) || nodeAnother.Type == model.NodeTransitPoint {
				allOnceNodes[nodeAnother.Id] = nodeAnother
			}
		}
		return allOnceNodes
	}

	allNodes = getConnectNodeOnce(node, allNodes)
	sum := planer.devManager.SystemConfig.TrafficCfg.TaskAvoidCfg.NearCalculateNumber
	// //优先找可避让属性的待命点和避让点
	// allCanGoList := planer.GetAllCanAvoidWaitingOrAvoidNode(robot)
	// for _, tmpNodeDJ := range allCanGoList {
	// 	tmpNode := tmpNodeDJ.CurrentNode
	// 	if _, find := allOlds[tmpNode.Id]; find {
	// 		continue
	// 	}
	// 	allOlds[tmpNode.Id] = tmpNode.Id
	// 	if tmpNode.Id == current_id {
	// 		continue
	// 	}

	// 	if _, find := goalRejects[tmpNode.Id]; find {
	// 		continue
	// 	}
	// 	if _, find := freeAvoidRejects[tmpNode.Id]; find {
	// 		continue
	// 	}
	// 	if planer.IsDeadWayAreaAvoidCanNotGo(tmpNode.Id, current_id) || planer.IsDeadWayAreaAvoidCanNotIn(tmpNode.Id, current_id) {
	// 		continue
	// 	}
	// 	if conflict, sn := RotateConflictRect(float64(tmpNode.X), float64(tmpNode.Y), robot.Type.GetRobotRotateRadius(robot)); conflict {
	// 		log.Infof("%s 不可达，robot:%s与robot:%s矩形或路径碰撞", tmpNode.Id, robot.Account.AgvId, sn)
	// 		continue
	// 	}
	// 	_, _, errPlan, blockAgv := planer.planOncePro(ctx, robot, robot.Account.MapName, robot.Status.SiteIdS, tmpNode.Id, tmpNode.Id, globalRejects, globalRejectsAll, localRejects, false, false, false)
	// 	// if curPaths != nil && curPaths.Paths != nil && gPaths != nil {
	// 	// _, isConflict, _ := gPaths.Conflict(curPaths.Paths.Back())
	// 	// 	if isConflict {
	// 	// 		continue
	// 	// 	}
	// 	// }
	// 	if r := robotmanager.GetFromCache(blockAgv); r.StatusIsExists() && r.Status.Status == model.RobotStatusIdle {

	// 	} else {
	// 		if errPlan != nil {
	// 			continue
	// 		}
	// 	}
	// 	return tmpNode
	// }
	for i := 0; i < sum; i++ {
		allNodesTmp := make(map[string]model.Node, 0)
		for _, tmpNode := range allNodes {
			for _, tmp0 := range getConnectNodeOnce(tmpNode, allNodes) {
				allNodesTmp[tmp0.Id] = tmp0
			}
		}
		if len(allNodes) == len(allNodesTmp) {
			break
		}
		allNodes = allNodesTmp
		for _, tmpNode := range allNodes {
			if tmpNode.Id == current_id {
				continue
			}
			if _, find := goalRejects[tmpNode.Id]; find {
				continue
			}
			if _, find := freeAvoidRejects[tmpNode.Id]; find {
				continue
			}
			if planer.IsDeadWayAreaAvoidCanNotGo(tmpNode.Id, current_id) || planer.IsDeadWayAreaAvoidCanNotIn(tmpNode.Id, current_id) {
				continue
			}
			if conflict, _ := RotateConflictRect(float64(tmpNode.X), float64(tmpNode.Y), robot.Type.GetRobotRotateRadius(robot)); conflict {
				continue
			}
			if curPaths, _, _ := planer.GlobalPathPlan(ctx, robot, robot.Status.SiteIdS, tmpNode.Id, tmpNode.Id, globalRejects, false, false, false); curPaths != nil && curPaths.Paths != nil && gPaths != nil {
				if pathEndBlockGlobalPath(tmpNode, angles.Deg2Rad(float64(curPaths.Paths.Back().PathEndAng)), gPaths) {
					continue
				}
			}
			log.Infof("robot:%s避让规划点位%s ", robot.Account.AgvId, tmpNode.Id)
			_, lPaths, errPlan, blockAgv := planer.planOncePro(ctx, robot, robot.Account.MapName, robot.Status.SiteIdS, tmpNode.Id, tmpNode.Id, globalRejects, globalRejectsAll, localRejects, false, false, true)
			blockAgvIdle := false
			if blockAgv != "" && !robotmanager.IsAgvHaveTask(ctx, blockAgv) {
				blockAgvIdle = true
			}
			if !blockAgvIdle && errPlan != nil {
				localRejects[tmpNode.Id] = tmpNode.Id
				delete(allNodes, tmpNode.Id)
			}
			if blockAgvIdle {

			} else {
				if errPlan != nil {
					continue
				} else if lPaths == nil || lPaths.Paths == nil {
					continue
				}
			}
			return tmpNode
		}
	}
	return model.Node{}
}
func (planer *Planer) getAgvFrontAvoidNode(ctx context.Context, robot *model.Robot, globalRejectsAll map[string]string) (string, []string) {
	node := planer.m.MapData.NodesMap[robot.Status.SiteIdS]
	getFrontNode := func(start_node model.Node) (model.Node, string) {
		nodeConnectPaths := planer.GetConnectPathsInStart(start_node)
		for _, nodePath := range nodeConnectPaths {
			nodeAnother := nodePath.GetAnother(start_node)
			if (nodePath.Dir == model.DirectionA2B && nodePath.BodyDirec == model.HeadBA) || (nodePath.Dir == model.DirectionB2A && nodePath.BodyDirec == model.HeadAB) {
				continue
			}
			dtheta := math.Abs(angles.NormalizeAngle(math.Atan2(float64(nodeAnother.Y-start_node.Y), float64(nodeAnother.X-start_node.X)) - angles.Deg2Rad(float64(robot.Status.Angle))))
			if dtheta < angles.Deg2Rad(10.0) && nodeAnother.Type == model.NodePath {
				return nodeAnother, nodePath.PathId
			}
		}
		return start_node, ""
	}
	var front_node model.Node
	var front_path string
	paths := make([]string, 0)
	current_node := node
	for i := 0; i < 5; i++ {
		front_node, front_path = getFrontNode(current_node)
		if front_node.Id == current_node.Id {
			return current_node.Id, paths
		} else {
			isInDeadArea := planer.IsDeadWayAreaAvoidCanNotIn(front_node.Id, node.Id)
			if !isInDeadArea {
				goalReachable, _ := planer.IsSiteReachable(ctx, robot, robot.Account.MapName, front_node.Id, globalRejectsAll, false)
				if !goalReachable {
					return current_node.Id, paths
				} else {
					if front_node.Distance(node) > 2*float64(robot.Type.GetRobotRotateRadius(robot)) {
						if front_path != "" {
							paths = append(paths, front_path)
						}
						return front_node.Id, paths
					}
				}
			} else {
				return current_node.Id, paths
			}
			if front_path != "" {
				paths = append(paths, front_path)
			}
			current_node = front_node
		}
	}
	return front_node.Id, paths
}

func (planer *Planer) getAgvBackAvoidNode(ctx context.Context, robot *model.Robot, globalRejectsAll map[string]string) (string, []string) {
	node := planer.m.MapData.NodesMap[robot.Status.SiteIdS]
	getBackNode := func(start_node model.Node) (model.Node, string) {
		nodeConnectPaths := planer.GetConnectPathsInStart(start_node)
		for _, nodePath := range nodeConnectPaths {
			nodeAnother := nodePath.GetAnother(start_node)
			dtheta := math.Abs(angles.NormalizeAngle(math.Atan2(float64(nodeAnother.Y-start_node.Y), float64(nodeAnother.X-start_node.X)) - angles.Deg2Rad(float64(robot.Status.Angle)+180.0)))
			if dtheta < angles.Deg2Rad(10.0) && nodePath.AllowBack == 1 && (nodePath.BodyDirec == model.Head ||
				(nodePath.Dir == model.DirectionA2B && nodePath.BodyDirec == model.HeadBA) ||
				(nodePath.Dir == model.DirectionB2A && nodePath.BodyDirec == model.HeadAB)) {
				return nodeAnother, nodePath.PathId
			}
		}
		return start_node, ""
	}
	var back_node model.Node
	var back_path string
	paths := make([]string, 0)
	current_node := node
	for i := 0; i < 5; i++ {
		back_node, back_path = getBackNode(current_node)
		if back_node.Id == current_node.Id {
			return current_node.Id, paths
		}
		if back_node.Type != model.NodePath && back_node.Type != model.NodeAvoid && back_node.Type != model.NodeTransitPoint && back_node.Type != model.NodeWaiting {
			return current_node.Id, make([]string, 0, 0)
		}
		goalReachable, _ := planer.IsSiteReachable(ctx, robot, robot.Account.MapName, back_node.Id, globalRejectsAll, false)
		if !goalReachable {
			return current_node.Id, paths
		} else {
			if back_node.Distance(node) > 4*float64(robot.Type.GetRobotRotateRadius(robot)) {
				if back_path != "" {
					paths = append(paths, back_path)
				}
				return back_node.Id, paths
			}
		}
		if back_path != "" {
			paths = append(paths, back_path)
		}
		current_node = back_node
	}
	return back_node.Id, paths
}

func (planer *Planer) getBestGlobalPathCost(ctx context.Context, robot *model.Robot, source, target string, rejects map[string]string) (float64, codes.Code, *model.TargetCarPose) {
	g0, errGetMap0 := planer.makeDijkstraMap(ctx, robot, target, rejects)
	if errGetMap0 != nil {
		log.Errorf("get dijkstra graph error:%s", errGetMap0.Error())
		return -1, codes.ErrPathPlanError.Wrap(fmt.Errorf("no path")), nil
	}
	sourceStr1 := fmt.Sprintf("%s:%s", source, source)
	targetStr1 := fmt.Sprintf("%s:%s", target, target)
	bestPath1, cost, err := g0.Shortest(robot.Account.AgvId, sourceStr1, targetStr1)
	if err != nil {
		return -1, codes.ErrPathPlanError.Wrap(err), nil
	}
	targetcarBody := new(model.TargetCarPose)
	if source != target {
		global_ids := make([]string, 0)
		for _, v := range bestPath1 {
			if v == sourceStr1 || v == targetStr1 {
				continue
			}
			id := strings.Split(v, ":")[1]
			global_ids = append(global_ids, id)
		}
		if len(global_ids) >= 2 {
			back_id := global_ids[len(global_ids)-1]
			back_front_id := global_ids[len(global_ids)-2]
			l := planer.m.MapData.Paths[back_front_id+"-"+back_id]
			//得到路径方向
			targetcarBody.CarBodyRad = l.BodyThetaRad()
			targetcarBody.PathID = l.PathId
			targetcarBody.TargetSiteId = back_id
		}
	}

	return cost, nil, targetcarBody

}
func (planer *Planer) GetBestGlobalPathFullCost(ctx context.Context, robot *model.Robot, source, target string, rejects map[string]string) (*model.GlobalPath, codes.Code, *model.TargetCarPose) {
	g0, errGetMap0 := planer.makeDijkstraMap(ctx, robot, target, rejects)
	if errGetMap0 != nil {
		log.Errorf("get dijkstra graph error:%s", errGetMap0.Error())
		return nil, codes.ErrPathPlanError.Wrap(fmt.Errorf("no path")), nil
	}
	sourceStr1 := fmt.Sprintf("%s:%s", source, source)
	targetStr1 := fmt.Sprintf("%s:%s", target, target)
	bestPath1, _, err := g0.Shortest(robot.Account.AgvId, sourceStr1, targetStr1)
	if err != nil {
		return nil, codes.ErrPathPlanError.Wrap(err), nil
	}
	targetcarBody := new(model.TargetCarPose)
	global_ids := make([]string, 0)
	if source != target {
		for _, v := range bestPath1 {
			if v == sourceStr1 || v == targetStr1 {
				continue
			}
			id := strings.Split(v, ":")[1]
			global_ids = append(global_ids, id)
		}
		if len(global_ids) >= 2 {
			back_id := global_ids[len(global_ids)-1]
			back_front_id := global_ids[len(global_ids)-2]
			l := planer.m.MapData.Paths[back_front_id+"-"+back_id]
			//得到路径方向
			targetcarBody.CarBodyRad = l.BodyThetaRad()
			targetcarBody.PathID = l.PathId
			targetcarBody.TargetSiteId = back_id
		}
	} else {
		targetcarBody.TargetSiteId = robot.Status.SiteIdS
		targetcarBody.PathID = ""
		targetcarBody.CarBodyRad = float64(robot.Status.Angle) * math.Pi / 180
	}
	gp, pathErr := planer.calculateGlobalPathCopy(ctx, robot, target, global_ids, false)
	if pathErr != nil {
		return nil, nil, targetcarBody
	}
	return gp, nil, targetcarBody
}

func (planer *Planer) getGlobalPath(ctx context.Context, robot *model.Robot, source, target string, rejects map[string]string) (*model.GlobalPath, []string) {
	g, errGetMap := planer.makeDijkstraMap(ctx, robot, target, rejects)
	if errGetMap != nil {
		log.Errorf("get dijkstra graph error:%s", errGetMap.Error())
		return nil, nil
	}
	sourceStr := fmt.Sprintf("%s:%s", source, source)
	targetStr := fmt.Sprintf("%s:%s", target, target)
	bestPath, _, errPlan := g.Shortest(robot.Account.AgvId, sourceStr, targetStr)
	if errPlan != nil {
		return nil, nil
	}
	ids := make([]string, 0)
	for _, v := range bestPath {
		if v == sourceStr || v == targetStr {
			continue
		}
		id := strings.Split(v, ":")[1]
		ids = append(ids, id)
	}
	gp, pathErr := planer.calculateGlobalPathCopy(ctx, robot, target, ids, false)
	if pathErr != nil {
		return nil, nil
	}
	return gp, ids
}
func (planer *Planer) getBestGlobalPath(ctx context.Context, robot *model.Robot, source, target string, finalTarget string, rejectsAll map[string]string, walkBack, needDelReverse bool) (gPath *model.GlobalPath, err error, blockAgv string) {
	g, errGetMap := planer.makeFullDijkstraMap(ctx, robot, target, rejectsAll, walkBack, needDelReverse)
	if errGetMap != nil {
		log.Errorf("get dijkstra graph error:%s", errGetMap.Error())
		return nil, errGetMap, ""
	}
	sourceStr := fmt.Sprintf("%s:%s", source, source)
	targetStr := fmt.Sprintf("%s:%s", target, target)
	bestPath, _, errPlan := g.Shortest(robot.Account.AgvId, sourceStr, targetStr)
	if errPlan != nil {
		if errors.Is(errPlan, dijkstra.ErrNoPath) {
			log.Errorf("g.Shortest err :%s", codes.ErrPathPlanNoGlobalPath.Error())
			return nil, codes.ErrPathPlanNoGlobalPath, ""
		}
		log.Errorf("get shortest path error:%s, robot:%s,source:%d,target:%d", errPlan.Error(), robot.Account.AgvId, source, target)
		return nil, errPlan, ""
	}
	ids := make([]string, 0)
	for _, v := range bestPath {
		if v == sourceStr || v == targetStr {
			continue
		}
		id := strings.Split(v, ":")[1]
		ids = append(ids, id)
	}
	gp, pathErr, blockAgv := planer.calculateGlobalPath(ctx, robot, finalTarget, ids, walkBack)
	if pathErr != nil {
		return nil, pathErr, blockAgv
	}
	return gp, nil, ""
}
func (planer *Planer) GetRobotStatusTaskFromDB(ctx context.Context, agvID string, status []model.TaskStatus) (*model.Task, codes.Code) {
	dbRes := planer.db.Collection(ctx, model.Task{}.TableName()).FindOne(ctx, bson.M{"agv_id": agvID, "status": bson.M{"$in": status}})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "get robot:%s running task from db error:%s", agvID, dbRes.Err().Error())
		return nil, codes.ErrDatabase.Wrap(dbRes.Err())
	}
	if dbRes.Err() != nil && errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		//log.Errorf("can not find robot running:%s task",agvID)
		return nil, codes.ErrTaskNotExists
	}
	task := new(model.Task)
	if errBind := dbRes.Decode(task); errBind != nil {
		log.ErrorfWithContext(ctx, "bind robot running task from db error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return task, nil
}
func (planer *Planer) makeFullDijkstraMap(ctx context.Context, robot *model.Robot, target string, rejects map[string]string, walkBack, needDelReverse bool) (*dijkstra.Graph, codes.Code) {
	robotNotInMapList := func(mapName string) bool {
		for _, perMap := range planer.devManager.SystemConfig.TrafficCfg.PathMutexMapNames {
			if perMap == mapName {
				return false
			}
		}
		return true
	}
	getRobotDirPaths := func(curMap *model.Map, theta float64, agvCorverPaths []string) (curP []model.Path) {
		curP = make([]model.Path, 0)
		handlePath := make(map[string]string, 0)
		for _, path := range agvCorverPaths {
			if _, find := handlePath[path]; !find {
				if p, findPath := curMap.MapData.Paths[path]; findPath {
					if p.Type == model.PathTypeLine && math.Abs(angles.NormalizeAngle(p.BodyThetaRad()-theta)) > math.Pi*35/36.0 {
						curP = append(curP, p)
					} else if p.Type == model.PathTypeBezier {
						curP = append(curP, p)
					}
				}
			}
		}
		return curP
	}

	if !robot.StatusIsExists() {
		return nil, codes.ErrDeviceStatusNotExists
	}
	robotsPath := make(map[string][]model.Path, 0)
	if needDelReverse {
		for _, r := range robotmanager.GetAllRobotsInSameMapFromCache(robot.Account.MapName) {
			if !r.StatusIsExists() || r.Account.AgvId == robot.Account.AgvId {
				continue
			}
			haveTask := false
			if r.Status.Status == model.RobotStatusIdle || r.Status.Status == model.RobotStatusStandby {
				if task, err := planer.GetRobotStatusTaskFromDB(ctx, r.Account.AgvId, model.TaskRunningStatus()); err == nil && task != nil {
					haveTask = true
				}
			} else {
				haveTask = true
			}
			if haveTask {
				robotDirPs := getRobotDirPaths(planer.m, angles.Deg2Rad(float64(r.Status.Angle)), r.AgvCorverPaths)
				if gPath, _ := planer.GetRobotGlobalPaths(context.Background(), r.Account.AgvId); gPath != nil && gPath.Paths != nil {
					total := r.LocalPathNum + 4
					for index, p := range gPath.Paths {
						if index > total {
							if p.RotateType != 0 || p.Type == model.PathTypeBezier || index > total+10 {
								break
							}
						}
						if p.Dir == model.DirectionA2B {
							p.Dir = model.DirectionB2A
						} else {
							p.Dir = model.DirectionA2B
						}
						robotDirPs = append(robotDirPs, p.Path)
					}
				} else {
					tmpPaths := planer.GetConnectPathsInStart(planer.GetNode(r.Status.SiteIdS))
					for _, p := range tmpPaths {
						if p.Type == model.PathTypeLine {
							if math.Abs(angles.NormalizeAngle(p.BodyThetaRad()-angles.Deg2Rad(float64(r.Status.Angle)))) < math.Pi/18 {
								continue
							} else if math.Abs(angles.NormalizeAngle(p.BodyThetaRad()-angles.Deg2Rad(float64(r.Status.Angle)+180.0))) < math.Pi/18 {
								continue
							}

						} else if p.Type == model.PathTypeBezier {
							_, end := p.BodyBezierThetaRad()
							if math.Abs(angles.NormalizeAngle(end-angles.Deg2Rad(float64(r.Status.Angle)))) < math.Pi/18 {
								continue
							} else if math.Abs(angles.NormalizeAngle(end-angles.Deg2Rad(float64(r.Status.Angle)+180.0))) < math.Pi/18 {
								continue
							}
						}
						if p.Dir == model.DirectionA2B {
							p.Dir = model.DirectionB2A
						} else {
							p.Dir = model.DirectionA2B
						}
						robotDirPs = append(robotDirPs, p)
					}
				}
				if len(robotDirPs) > 0 {
					robotsPath[r.Account.AgvId] = robotDirPs
				}
			}
		}
	}
	graphRaw, findGraphRaw := planer.graphs[robot.Type.CategoryId]
	if !findGraphRaw {
		graphRaw = planer.initDijkstraMap(robot.Type)
	}
	//拷贝代价地图
	graph := graphRaw.DeepCopy()
	robotThetaRad := angles.Deg2Rad(float64(robot.Status.Angle))
	//移除剔除点
	for rej, _ := range rejects {
		//log.Infof("remove vertex:%d", rej)
		graph.RemoveVertex(fmt.Sprint(rej))
	}
	//剔除全局路径中已规划方向路径的反向路径
	allGlobalPaths, errGetAll := planer.getAllGlobalPaths(ctx)
	if errGetAll != nil {
		return nil, errGetAll
	}
	addEdge := func(p *model.TrafficPath, theta float64, s, t string, lastIsTailGo bool) {
		//代价值，旋转代价为nodeAPath的旋转代价
		cost := planer.getPathCostFunc(robot.Account.AgvId, robot, p, theta, robot.Type, lastIsTailGo)
		graph.AddEdge(s, t, cost)
	}

	//获得所有允许路线对冲区域
	allPathMutesAreas := planer.GetAreaByAreaType(model.AreaTypeAllowPathMutex)
	//获得所有路径允许对冲区域中的点
	allPathMutexContainsMap := make(map[string]string, 0)
	for _, pathMutexArea := range allPathMutesAreas {
		if len(pathMutexArea.ContainDotIds) == 0 {
			continue
		}
		for _, containId := range pathMutexArea.ContainDotIds {
			allPathMutexContainsMap[containId] = containId
		}
	}

	for _, l := range planer.m.MapData.Paths {

		//过滤已经剔除的路
		if _, find := rejects[l.NodeA.Id]; find {
			continue
		}
		if _, find := rejects[l.NodeB.Id]; find {
			continue
		}

		//如果该路径在允许路径对冲范围继续
		_, isAllowPathMutex1 := allPathMutexContainsMap[l.NodeA.Id]
		_, isAllowPathMutex2 := allPathMutexContainsMap[l.NodeB.Id]
		isAllowPathMutex := isAllowPathMutex1 || isAllowPathMutex2

		if !isAllowPathMutex && needDelReverse {
			for _, robotPath := range robotsPath {
				for _, g := range robotPath {
					if g.Id == l.Id && g.Dir == l.Dir {
						if g.Dir == model.DirectionB2A {
							// log.Infof("remove edge:%s to %s", l.NodeB.Id, l.NodeA.Id)
							graph.RemoveEdge(fmt.Sprint(l.NodeB.Id), fmt.Sprint(l.NodeA.Id))
							continue
						}
						if g.Dir == model.DirectionA2B {
							// log.Infof("remove edge:%s to %s", l.NodeA.Id, l.NodeB.Id)
							graph.RemoveEdge(fmt.Sprint(l.NodeA.Id), fmt.Sprint(l.NodeB.Id))
							continue
						}
					}
				}
			}
		}
		//方向
		notA2B := false
		notB2A := false
		//判断不允许路线对冲情况下
		if (!planer.devManager.SystemConfig.TrafficCfg.UsePathMutex || robotNotInMapList(robot.Account.MapName)) && !isAllowPathMutex {
			for _, g := range allGlobalPaths {
				if g.Sn == robot.Account.AgvId {
					continue
				}
				if p, findPath := g.Paths.HasPath(l.PathId); findPath {
					//如果路已经存在，去掉它的反向(有可能多次)
					notA2B = notA2B || p.Dir == model.DirectionB2A
					notB2A = notB2A || p.Dir == model.DirectionA2B
				}
			}
			if notA2B && l.Dir == model.DirectionA2B {
				//log.Infof("remove edge:%d to %d", l.NodeA.Id, l.NodeB.Id)
				graph.RemoveEdge(fmt.Sprint(l.NodeA.Id), fmt.Sprint(l.NodeB.Id))
				continue
			}
			if notB2A && l.Dir == model.DirectionB2A {
				//log.Infof("remove edge:%d to %d", l.NodeB.Id, l.NodeA.Id)
				graph.RemoveEdge(fmt.Sprint(l.NodeB.Id), fmt.Sprint(l.NodeA.Id))
				continue
			}
		}

		//机器人当前位置为起点，所有与其相连的路径的终点为终点
		robotA := false //机器人在nodeA
		robotB := false //机器人在nodeB
		nodeA := l.NodeA
		nodeB := l.NodeB
		edgeCheck := make(map[string]struct{})
		var costStart func(sn string) float64
		if l.GetSource().Id == robot.Status.SiteIdS {
			if robot.Status.SiteIdS == nodeA.Id {
				robotA = true
			}
			if robot.Status.SiteIdS == nodeB.Id {
				robotB = true
			}
			costStart = planer.getPathCostFunc(robot.Account.AgvId, robot, &model.TrafficPath{Path: l}, robotThetaRad, robot.Type)
		}
		trafficL := &model.TrafficPath{Path: l}
		//终点代价地图点
		if !notA2B && l.Dir == model.DirectionA2B && nodeB.Id == target {
			//nodeB为最终终点
			//以nodeA为终点的所有路段
			nodeAConnectPaths := planer.getConnectPathsNot(l.NodeA, l.PathId) //与该路段nodeA相连的所有路段
			t := fmt.Sprintf("%s-%s:%s", nodeA.Id, nodeB.Id, nodeB.Id)
			graph.AddEdge(t, fmt.Sprintf("%s:%s", nodeB.Id, nodeB.Id), func(sn string) float64 { return 0.0 }) //串联
			for _, nodeAPath := range nodeAConnectPaths {
				if nodeAPath.GetTarget().Id != nodeA.Id {
					continue
				}
				//代价值，旋转代价为nodeAPath的旋转代价
				s := fmt.Sprintf("%s-%s-%s:%s", nodeAPath.GetAnother(nodeA).Id, nodeA.Id, nodeB.Id, nodeA.Id)
				target_theta := nodeAPath.BodyThetaRad()
				if nodeAPath.Type == model.PathTypeBezier {
					_, target_theta = nodeAPath.BodyBezierThetaRad()
				}
				//判断上个路径是用头走还是尾走
				lastIsTailGo := false
				if nodeAPath.BodyDirec == model.HeadBA && nodeAPath.GetSource().Id == nodeAPath.NodeA.Id {
					lastIsTailGo = true

				} else if nodeAPath.BodyDirec == model.HeadAB && nodeAPath.GetSource().Id == nodeAPath.NodeB.Id {
					lastIsTailGo = true
				}
				addEdge(trafficL, target_theta, s, t, lastIsTailGo)
			}
			//机器人在nodeA
			if robotA {
				s := fmt.Sprintf("%s-%s:%s", nodeA.Id, nodeB.Id, nodeA.Id)
				graph.AddEdge(s, t, costStart)
				graph.AddEdge(fmt.Sprintf("%s:%s", nodeA.Id, nodeA.Id), s, func(sn string) float64 { return 0.0 }) //串联
			}
		}
		if !notB2A && l.Dir == model.DirectionB2A && nodeA.Id == target {
			//nodeA为断头路终点
			//以nodeB为终点的所有路段
			nodeBConnectPaths := planer.getConnectPathsNot(l.NodeB, l.PathId) //与该路段nodeB相连的所有路段
			t := fmt.Sprintf("%s-%s:%s", nodeB.Id, nodeA.Id, nodeA.Id)
			graph.AddEdge(t, fmt.Sprintf("%s:%s", nodeA.Id, nodeA.Id), func(sn string) float64 { return 0.0 }) //串联
			for _, nodeBPath := range nodeBConnectPaths {
				if nodeBPath.GetTarget().Id != nodeB.Id {
					continue
				}
				//代价值，旋转代价为nodeBPath的旋转代价
				s := fmt.Sprintf("%s-%s-%s:%s", nodeBPath.GetAnother(nodeB).Id, nodeB.Id, nodeA.Id, nodeB.Id)
				target_theta := nodeBPath.BodyThetaRad()
				if nodeBPath.Type == model.PathTypeBezier {
					_, target_theta = nodeBPath.BodyBezierThetaRad()
				}
				//
				//判断上个路径是用头走还是尾走
				lastIsTailGo := false
				if nodeBPath.BodyDirec == model.HeadBA && nodeBPath.GetSource().Id == nodeBPath.NodeA.Id {
					lastIsTailGo = true

				} else if nodeBPath.BodyDirec == model.HeadAB && nodeBPath.GetSource().Id == nodeBPath.NodeB.Id {
					lastIsTailGo = true
				}
				addEdge(trafficL, target_theta, s, t, lastIsTailGo)
			}
			//机器人在nodeB
			if robotB {
				s := fmt.Sprintf("%s-%s:%s", nodeB.Id, nodeA.Id, nodeB.Id)
				graph.AddEdge(s, t, costStart)
				graph.AddEdge(fmt.Sprintf("%s:%s", nodeB.Id, nodeB.Id), s, func(sn string) float64 { return 0.0 }) //串联
			}
		}
		//===
		if !robotA && !robotB {
			//机器人不在该路径上，跳过
			continue
		}
		nodeAConnectPaths := planer.getConnectPathsNot(l.NodeA, l.PathId) //与该路段nodeA相连的所有路段
		nodeBConnectPaths := planer.getConnectPathsNot(l.NodeB, l.PathId) //与该路段nodeB相连的所有路段
		//==
		for _, nodeAPath := range nodeAConnectPaths {
			//机器人在nodeB，将以nodeA为起点的点加入代价地图
			if robotB && nodeAPath.GetSource().Id == nodeA.Id {
				s := fmt.Sprintf("%s-%s:%s", nodeB.Id, nodeA.Id, nodeB.Id)
				t := fmt.Sprintf("%s-%s-%s:%s", nodeB.Id, nodeA.Id, nodeAPath.GetAnother(nodeA).Id, nodeA.Id)
				graph.AddEdge(fmt.Sprintf("%s:%s", nodeB.Id, nodeB.Id), s, func(sn string) float64 { return 0.0 }) //串联
				graph.AddEdge(s, t, costStart)
			}
			for _, nodeBPath := range nodeBConnectPaths {
				//机器人在nodeA，将以nodeB为起点的点加入代价地图
				if robotA && nodeBPath.GetSource().Id == nodeB.Id {
					s := fmt.Sprintf("%s-%s:%s", nodeA.Id, nodeB.Id, nodeA.Id)
					t := fmt.Sprintf("%s-%s-%s:%s", nodeA.Id, nodeB.Id, nodeBPath.GetAnother(nodeB).Id, nodeB.Id)
					_, find := edgeCheck[t]
					if !find {
						graph.AddEdge(fmt.Sprintf("%s:%s", nodeA.Id, nodeA.Id), s, func(sn string) float64 { return 0.0 }) //串联
						graph.AddEdge(s, t, costStart)
						edgeCheck[t] = struct{}{}
					}
				}
			}
		}
		//机器人在断头路出发
		if robotA && len(nodeAConnectPaths) == 0 {
			s := fmt.Sprintf("%s-%s:%s", nodeA.Id, nodeB.Id, nodeA.Id)
			for _, nodeBPath := range nodeBConnectPaths {
				t := fmt.Sprintf("%s-%s-%s:%s", nodeA.Id, nodeB.Id, nodeBPath.GetAnother(nodeB).Id, nodeB.Id)
				graph.AddEdge(s, t, costStart)
			}
			graph.AddEdge(fmt.Sprintf("%s:%s", nodeA.Id, nodeA.Id), s, func(sn string) float64 { return 0.0 }) //串联
		}
		if robotB && len(nodeBConnectPaths) == 0 {
			s := fmt.Sprintf("%s-%s:%s", nodeB.Id, nodeA.Id, nodeB.Id)
			for _, nodeAPath := range nodeAConnectPaths {
				t := fmt.Sprintf("%s-%s-%s:%s", nodeB.Id, nodeA.Id, nodeAPath.GetAnother(nodeA).Id, nodeA.Id)
				graph.AddEdge(s, t, costStart)
			}
			graph.AddEdge(fmt.Sprintf("%s:%s", nodeB.Id, nodeB.Id), s, func(sn string) float64 { return 0.0 }) //串联
		}
		//后退避让删除贝塞尔导致的路径计算错误
		if walkBack {
			if l.Type == model.PathTypeBezier {
				if l.Dir == model.DirectionA2B {
					graph.RemoveEdge(fmt.Sprint(l.NodeA.Id), fmt.Sprint(l.NodeB.Id))
				} else {
					graph.RemoveEdge(fmt.Sprint(l.NodeB.Id), fmt.Sprint(l.NodeA.Id))
				}
			}
		}
	}
	//对潮汐区域的处理
	planer.workAreasLocker.Lock()
	defer planer.workAreasLocker.Unlock()
	var localPaths []model.LocalPath
	retWorkAreas, err := planer.PlanerArea(ctx, robot.Account.AgvId, localPaths, false)
	if err != nil {

	}
	for _, a := range retWorkAreas {
		if a.Type == model.AreaTypeTide {
			// log.Errorf("----------------全局计算的潮汐对象方向:%d", a.LockMethod)
			for _, p := range a.ContainPathIds {
				if (p.Dir == model.DirectionA2B && a.LockMethod == 2) || (p.Dir == model.DirectionB2A && a.LockMethod == 1) {
					graph.RemoveEdge(fmt.Sprint(p.NodeA.Id), fmt.Sprint(p.NodeB.Id))
					continue
				}
				if (p.Dir == model.DirectionA2B && a.LockMethod == 1) || (p.Dir == model.DirectionB2A && a.LockMethod == 2) {
					graph.RemoveEdge(fmt.Sprint(p.NodeB.Id), fmt.Sprint(p.NodeA.Id))
					continue
				}
			}
		}
	}
	return graph, nil
}

func (planer *Planer) makeDijkstraMap(ctx context.Context, robot *model.Robot, target string, rejects map[string]string) (*dijkstra.Graph, codes.Code) {
	if !robot.StatusIsExists() {
		return nil, codes.ErrDeviceStatusNotExists
	}
	currentRobotSiteIds := robot.Status.SiteIdS

	graphRaw, findGraphRaw := planer.graphs[robot.Type.CategoryId]
	if !findGraphRaw {
		graphRaw = planer.initDijkstraMap(robot.Type)
	}
	//拷贝代价地图
	graph := graphRaw.DeepCopy()
	robotThetaRad := angles.Deg2Rad(float64(robot.Status.Angle))
	//移除剔除点
	for rej, _ := range rejects {
		graph.RemoveVertex(fmt.Sprint(rej))
	}
	addEdge := func(p *model.TrafficPath, theta float64, s, t string, lastIsTail bool) {
		//代价值，旋转代价为nodeAPath的旋转代价
		cost := planer.getPathCostFunc(robot.Account.AgvId, robot, p, theta, robot.Type, lastIsTail)
		graph.AddEdge(s, t, cost)
	}
	for _, l := range planer.m.MapData.Paths {
		//过滤已经剔除的路
		if _, find := rejects[l.NodeA.Id]; find {
			continue
		}
		if _, find := rejects[l.NodeB.Id]; find {
			continue
		}
		//机器人当前位置为起点，所有与其相连的路径的终点为终点
		robotA := false //机器人在nodeA
		robotB := false //机器人在nodeB
		nodeA := l.NodeA
		nodeB := l.NodeB
		edgeCheck := make(map[string]struct{})
		var costStart func(sn string) float64
		if l.GetSource().Id == currentRobotSiteIds {
			if currentRobotSiteIds == nodeA.Id {
				robotA = true
			}
			if currentRobotSiteIds == nodeB.Id {
				robotB = true
			}
			costStart = planer.getPathCostFunc(robot.Account.AgvId, robot, &model.TrafficPath{Path: l}, robotThetaRad, robot.Type)
		}
		trafficL := &model.TrafficPath{Path: l}
		//终点代价地图点
		if l.Dir == model.DirectionA2B && nodeB.Id == target {
			//nodeB为最终终点
			//以nodeA为终点的所有路段
			nodeAConnectPaths := planer.getConnectPathsNot(l.NodeA, l.PathId) //与该路段nodeA相连的所有路段
			t := fmt.Sprintf("%s-%s:%s", nodeA.Id, nodeB.Id, nodeB.Id)
			graph.AddEdge(t, fmt.Sprintf("%s:%s", nodeB.Id, nodeB.Id), func(sn string) float64 { return 0.0 }) //串联
			for _, nodeAPath := range nodeAConnectPaths {
				if nodeAPath.GetTarget().Id != nodeA.Id {
					continue
				}
				//代价值，旋转代价为nodeAPath的旋转代价
				s := fmt.Sprintf("%s-%s-%s:%s", nodeAPath.GetAnother(nodeA).Id, nodeA.Id, nodeB.Id, nodeA.Id)
				target_theta := nodeAPath.BodyThetaRad()
				if nodeAPath.Type == model.PathTypeBezier {
					_, target_theta = nodeAPath.BodyBezierThetaRad()
				}
				//判断上个路径是用头走还是尾走
				lastIsTailGo := false
				if nodeAPath.BodyDirec == model.HeadBA && nodeAPath.GetSource().Id == nodeAPath.NodeA.Id {
					lastIsTailGo = true

				} else if nodeAPath.BodyDirec == model.HeadAB && nodeAPath.GetSource().Id == nodeAPath.NodeB.Id {
					lastIsTailGo = true
				}
				addEdge(trafficL, target_theta, s, t, lastIsTailGo)
			}
			//机器人在nodeA
			if robotA {
				s := fmt.Sprintf("%s-%s:%s", nodeA.Id, nodeB.Id, nodeA.Id)
				graph.AddEdge(s, t, costStart)
				graph.AddEdge(fmt.Sprintf("%s:%s", nodeA.Id, nodeA.Id), s, func(sn string) float64 { return 0.0 }) //串联
			}
		}
		if l.Dir == model.DirectionB2A && nodeA.Id == target {
			//nodeA为断头路终点
			//以nodeB为终点的所有路段
			nodeBConnectPaths := planer.getConnectPathsNot(l.NodeB, l.PathId) //与该路段nodeB相连的所有路段
			t := fmt.Sprintf("%s-%s:%s", nodeB.Id, nodeA.Id, nodeA.Id)
			graph.AddEdge(t, fmt.Sprintf("%s:%s", nodeA.Id, nodeA.Id), func(sn string) float64 { return 0.0 }) //串联
			for _, nodeBPath := range nodeBConnectPaths {
				if nodeBPath.GetTarget().Id != nodeB.Id {
					continue
				}
				//代价值，旋转代价为nodeBPath的旋转代价
				s := fmt.Sprintf("%s-%s-%s:%s", nodeBPath.GetAnother(nodeB).Id, nodeB.Id, nodeA.Id, nodeB.Id)
				target_theta := nodeBPath.BodyThetaRad()
				if nodeBPath.Type == model.PathTypeBezier {
					_, target_theta = nodeBPath.BodyBezierThetaRad()
				}
				//判断上个路径是用头走还是尾走
				lastIsTailGo := false
				if nodeBPath.BodyDirec == model.HeadBA && nodeBPath.GetSource().Id == nodeBPath.NodeA.Id {
					lastIsTailGo = true

				} else if nodeBPath.BodyDirec == model.HeadAB && nodeBPath.GetSource().Id == nodeBPath.NodeB.Id {
					lastIsTailGo = true
				}
				addEdge(trafficL, target_theta, s, t, lastIsTailGo)
			}
			//机器人在nodeB
			if robotB {
				s := fmt.Sprintf("%s-%s:%s", nodeB.Id, nodeA.Id, nodeB.Id)
				graph.AddEdge(s, t, costStart)
				graph.AddEdge(fmt.Sprintf("%s:%s", nodeB.Id, nodeB.Id), s, func(sn string) float64 { return 0.0 }) //串联
			}
		}
		//===
		if !robotA && !robotB {
			//机器人不在该路径上，跳过
			continue
		}
		nodeAConnectPaths := planer.getConnectPathsNot(l.NodeA, l.PathId) //与该路段nodeA相连的所有路段
		nodeBConnectPaths := planer.getConnectPathsNot(l.NodeB, l.PathId) //与该路段nodeB相连的所有路段
		//==
		for _, nodeAPath := range nodeAConnectPaths {
			//机器人在nodeB，将以nodeA为起点的点加入代价地图
			if robotB && nodeAPath.GetSource().Id == nodeA.Id {
				s := fmt.Sprintf("%s-%s:%s", nodeB.Id, nodeA.Id, nodeB.Id)
				t := fmt.Sprintf("%s-%s-%s:%s", nodeB.Id, nodeA.Id, nodeAPath.GetAnother(nodeA).Id, nodeA.Id)
				graph.AddEdge(fmt.Sprintf("%s:%s", nodeB.Id, nodeB.Id), s, func(sn string) float64 { return 0.0 }) //串联
				graph.AddEdge(s, t, costStart)
			}
			for _, nodeBPath := range nodeBConnectPaths {
				//机器人在nodeA，将以nodeB为起点的点加入代价地图
				if robotA && nodeBPath.GetSource().Id == nodeB.Id {
					s := fmt.Sprintf("%s-%s:%s", nodeA.Id, nodeB.Id, nodeA.Id)
					t := fmt.Sprintf("%s-%s-%s:%s", nodeA.Id, nodeB.Id, nodeBPath.GetAnother(nodeB).Id, nodeB.Id)
					_, find := edgeCheck[t]
					if !find {
						graph.AddEdge(fmt.Sprintf("%s:%s", nodeA.Id, nodeA.Id), s, func(sn string) float64 { return 0.0 }) //串联
						graph.AddEdge(s, t, costStart)
						edgeCheck[t] = struct{}{}
					}
				}
			}
		}
		//机器人在断头路出发
		if robotA && len(nodeAConnectPaths) == 0 {
			s := fmt.Sprintf("%s-%s:%s", nodeA.Id, nodeB.Id, nodeA.Id)
			for _, nodeBPath := range nodeBConnectPaths {
				t := fmt.Sprintf("%s-%s-%s:%s", nodeA.Id, nodeB.Id, nodeBPath.GetAnother(nodeB).Id, nodeB.Id)
				graph.AddEdge(s, t, costStart)
			}
			graph.AddEdge(fmt.Sprintf("%s:%s", nodeA.Id, nodeA.Id), s, func(sn string) float64 { return 0.0 }) //串联
		}
		if robotB && len(nodeBConnectPaths) == 0 {
			s := fmt.Sprintf("%s-%s:%s", nodeB.Id, nodeA.Id, nodeB.Id)
			for _, nodeAPath := range nodeAConnectPaths {
				t := fmt.Sprintf("%s-%s-%s:%s", nodeB.Id, nodeA.Id, nodeAPath.GetAnother(nodeA).Id, nodeA.Id)
				graph.AddEdge(s, t, costStart)
			}
			graph.AddEdge(fmt.Sprintf("%s:%s", nodeB.Id, nodeB.Id), s, func(sn string) float64 { return 0.0 }) //串联
		}
	}
	return graph, nil
}

//initDijkstraMap 初始化全图，除了起点，代价地图
func (planer *Planer) initDijkstraMap(robotType model.RobotType) *dijkstra.Graph {
	graph := dijkstra.NewGraph()
	//创建代价地图
	//第一部分：机器人当前位置为起点，所有与其相连的路径的终点为终点
	//第二部分：其它路段
	addEdge := func(p *model.TrafficPath, theta float64, s, t, start, index string, isLastTailGo bool) {
		//代价值，旋转代价为nodeAPath的旋转代价
		cost := planer.getPathCostFunc("", nil, p, theta, robotType, isLastTailGo)
		graph.AddEdgeAndInfo(s, t, start, index, cost, p)
	}
	startTime := time.Now()
	for _, l := range planer.m.MapData.Paths {
		nodeA := l.NodeA
		nodeB := l.NodeB
		trafficL := &model.TrafficPath{Path: l}
		//机器人当前位置为起点，所有与其相连的路径的终点为终点
		nodeAConnectPaths := planer.getConnectPathsNot(l.NodeA, l.PathId) //与该路段nodeA相连的所有路段
		nodeBConnectPaths := planer.getConnectPathsNot(l.NodeB, l.PathId) //与该路段nodeB相连的所有路段
		for _, nodeAPath := range nodeAConnectPaths {
			for _, nodeBPath := range nodeBConnectPaths {
				//A->B方向的路
				//即，以A为终点的nodeAPath -> 以B为起点的nodeBPath
				if l.Dir == model.DirectionA2B && (nodeAPath.GetTarget().Id == nodeA.Id && nodeBPath.GetSource().Id == nodeB.Id) {
					//代价值，旋转代价为nodeAPath的旋转代价
					s := fmt.Sprintf("%s-%s-%s:%s", nodeAPath.GetAnother(nodeA).Id, nodeA.Id, nodeB.Id, nodeA.Id)
					t := fmt.Sprintf("%s-%s-%s:%s", nodeA.Id, nodeB.Id, nodeBPath.GetAnother(nodeB).Id, nodeB.Id)
					start := nodeAPath.GetAnother(nodeA).Id
					index := fmt.Sprintf("%s->%s", nodeA.Id, nodeB.Id)
					target_theta := nodeAPath.BodyThetaRad()
					if nodeAPath.Type == model.PathTypeBezier {
						_, target_theta = nodeAPath.BodyBezierThetaRad()
					}
					//判断上个路径是用头走还是尾走
					lastIsTailGo := false
					if nodeBPath.BodyDirec == model.HeadBA && nodeAPath.GetSource().Id == nodeAPath.NodeA.Id {
						lastIsTailGo = true

					} else if nodeBPath.BodyDirec == model.HeadAB && nodeAPath.GetSource().Id == nodeAPath.NodeB.Id {
						lastIsTailGo = true

					}
					addEdge(trafficL, target_theta, s, t, start, index, lastIsTailGo)
				}
				//B->A方向
				//即，以B为终点的nodeBPath -> 以A为起点的nodeBPath
				if l.Dir == model.DirectionB2A && (nodeBPath.GetTarget().Id == nodeB.Id && nodeAPath.GetSource().Id == nodeA.Id) {
					//代价值，旋转代价为nodeBPath的旋转代价
					s := fmt.Sprintf("%s-%s-%s:%s", nodeBPath.GetAnother(nodeB).Id, nodeB.Id, nodeA.Id, nodeB.Id)
					t := fmt.Sprintf("%s-%s-%s:%s", nodeB.Id, nodeA.Id, nodeAPath.GetAnother(nodeA).Id, nodeA.Id)
					start := nodeBPath.GetAnother(nodeB).Id
					index := fmt.Sprintf("%s->%s", nodeB.Id, nodeA.Id)
					target_theta := nodeBPath.BodyThetaRad()
					if nodeBPath.Type == model.PathTypeBezier {
						_, target_theta = nodeBPath.BodyBezierThetaRad()
					}
					//判断上个路径是用头走还是尾走
					lastIsTailGo := false
					if nodeBPath.BodyDirec == model.HeadBA && nodeBPath.GetSource().Id == nodeBPath.NodeA.Id {
						lastIsTailGo = true

					} else if nodeBPath.BodyDirec == model.HeadAB && nodeBPath.GetSource().Id == nodeBPath.NodeB.Id {
						lastIsTailGo = true
					}
					addEdge(trafficL, target_theta, s, t, start, index, lastIsTailGo)
				}
			}
		}
	}
	fmt.Printf("map:%s robotType:%d handle time %dms\n", planer.m.MapName, robotType.CategoryId, time.Now().UnixMilli()-startTime.UnixMilli())
	planer.graphs[robotType.CategoryId] = graph
	return graph
}

func (planer *Planer) getConnectPathsNot(node model.Node, not string) []model.Path {
	res := make([]model.Path, 0)
	for _, p := range node.ConnectPaths {
		l := planer.m.MapData.Paths[p]
		if l.PathId == not {
			continue
		}
		res = append(res, l)
	}
	return res
}

func (planer *Planer) getConnectPaths(node model.Node) []model.Path {
	res := make([]model.Path, 0)
	for _, p := range node.ConnectPaths {
		l := planer.m.MapData.Paths[p]
		res = append(res, l)
	}
	return res
}
func (planer *Planer) GetConnectPathsInStart(node model.Node) []model.Path {
	res := make([]model.Path, 0)
	for _, p := range node.ConnectPaths {
		l := planer.m.MapData.Paths[p]
		if l.GetSource().Id == node.Id {
			res = append(res, l)
		}
	}
	return res
}

func (planer *Planer) IsConnectPathsNextIsStation(currentNode model.Node) bool {
	for _, p := range currentNode.ConnectPaths {
		l := planer.m.MapData.Paths[p]
		if l.GetSource().Id == currentNode.Id && (l.GetTarget().Type == model.NodeStation || l.GetTarget().Type == model.NodeConveyorLine ||
			l.GetTarget().Type == model.NodeManipulator || l.GetTarget().Type == model.NodeHoist || l.GetTarget().Type == model.NodeStacker ||
			l.GetTarget().Type == model.NodeDiscRemover || l.GetTarget().Type == model.NodeRgv || l.GetTarget().Type == model.NodePlatformWeight) {
			// res = append(res, l)
			//判断下一条路径
			return true
		}
	}
	return false
}
func (planer *Planer) IsCrossRoad(nodeId string) bool {
	res := make([]model.Path, 0)
	node := planer.GetNode(nodeId)
	if node.Type == model.NodeStation || node.Type == model.NodeWaiting || node.Type == model.NodeCharge {
		return false
	}
	for _, p := range node.ConnectPaths {
		l := planer.m.MapData.Paths[p]
		if l.GetSource().Id == nodeId && l.GetTarget().Type != model.NodeStation {
			paths := planer.getConnectPathsNot(l.GetTarget(), l.PathId)
			if len(paths) <= 2 && len(paths) > 0 {
				if paths[0].GetAnother(l.GetTarget()).Type == model.NodeStation {
					continue
				}
			}
			res = append(res, l)
		}
	}
	if len(res) > 2 {
		return true
	}
	return false
}
func (planer *Planer) blockWithObstacle(path *model.TrafficPath, radius float64) bool {
	isRotateConflict := false
	//inAngleRangeCheck 加入路径起点的旋转类型，判断targetTheta是否在startTheta，endTheta范围内,输入均为角度
	inAngleRangeChec := func(startTheta, endTheta, targetTheta, rotate_type int) bool {
		if startTheta > endTheta {
			if targetTheta < startTheta && targetTheta > endTheta {
				if rotate_type == -1 {
					return true
				}
			} else {
				if rotate_type == 1 {
					return true
				}
			}
		} else {
			if targetTheta < endTheta && targetTheta > startTheta {
				if rotate_type == 1 {
					return true
				}
			} else {
				if rotate_type == -1 {
					return true
				}
			}
		}
		return false
	}
	blockCircle := func(path *model.TrafficPath, p1, p2 model.Pose2, radius float64) (bool, bool) {
		node := path.Path.GetSource()
		x, y := float64(node.X), float64(node.Y)
		xa, ya, xb, yb := p1.X, p1.Y, p2.X, p2.Y
		line_dis := 99999.0
		nearest_x, nearest_y := 0.0, 0.0
		if (xb-xa)*(x-xa)+(yb-ya)*(y-ya) < 0 {
			line_dis = math.Sqrt((xa-x)*(xa-x) + (ya-y)*(ya-y))
			nearest_x, nearest_y = xa, ya
		} else if (xa-xb)*(x-xb)+(ya-yb)*(y-yb) < 0 {
			line_dis = math.Sqrt((xb-x)*(xb-x) + (yb-y)*(yb-y))
			nearest_x, nearest_y = xb, yb
		} else {
			if xa == xb {
				line_dis = math.Abs(x - xa)
				nearest_x, nearest_y = xa, y
			} else if ya == yb {
				line_dis = math.Abs(y - ya)
				nearest_x, nearest_y = x, ya
			} else {
				k := (yb - ya) / (xb - xa)
				b := yb - k*xb
				line_dis = math.Abs(k*x-y+b) / math.Sqrt(k*k+1)
				//两条垂直相交直线的斜率相乘积为-1：k1*k2=-1.
				// y0=k*x0+b 与 (y-y0)/(x-x0) = -1/k，已知x,y,求x0,y0
				nearest_x = (k*y + x - k*b) / (k*k + 1)
				nearest_y = k*nearest_x + b
			}
		}

		if line_dis < radius {
			theta := int(math.Atan2(nearest_y-y, nearest_x-x) * 180.0 / math.Pi)
			if theta < 0 {
				theta = theta + 360
			}
			if inAngleRangeChec(path.RotateStartAng, path.RotateEndAng, theta, path.RotateType) {
				return true, true
			}
			return true, false
		}
		return false, false
	}

	if path.RotateType != 0 {
		for _, area := range planer.m.MapData.Obstacle {
			edges := area.EdgePoints
			disBool := false
			for i := 0; i <= len(edges)-1; i++ {
				disBoolTemp, block := false, false
				if i == len(edges)-1 {
					disBoolTemp, block = blockCircle(path, edges[len(edges)-1], edges[0], radius)
				} else {
					disBoolTemp, block = blockCircle(path, edges[i], edges[i+1], radius)
				}
				if disBoolTemp {
					disBool = disBoolTemp
				}
				if block {
					isRotateConflict = true
					break
				}
			}
			if !isRotateConflict && disBool {
				delta := 5.0
				if path.RotateType == -1 {
					delta = -5.0
				}
				for i := float64(path.RotateStartAng); ; i += delta {
					if i > 360 {
						i -= 360
					}
					if i < 0 {
						i += 360
					}
					xi := float64(path.Path.GetSource().X) + radius*math.Cos(float64(i)*math.Pi/180.0)
					yi := float64(path.Path.GetSource().Y) + radius*math.Sin(float64(i)*math.Pi/180.0)
					if edges.PointInPolygon(xi, yi) {
						isRotateConflict = true
						break
					}
					dtheta := i - float64(path.RotateEndAng)
					if dtheta > 180 {
						dtheta -= 360
					}
					if dtheta < -180 {
						dtheta += 360
					}
					// log.Warnf("**************%f   %f", i, dtheta)
					if math.Abs(dtheta) <= 0.5*math.Abs(delta) {
						break
					}
				}
			}
			if isRotateConflict {
				if path.RotateType == 1 {
					log.Warnf("碰撞:障碍物%s多边形与节点%s逆时针旋转扇形碰撞", area.Id, path.Path.GetSource().Id)
				} else {
					log.Warnf("碰撞:障碍物%s多边形与节点%s顺时针旋转扇形碰撞", area.Id, path.Path.GetSource().Id)
				}
				break
			}
		}
	}
	return isRotateConflict
}

//todo 与getPathCost函数合并
func (planer *Planer) calculateGlobalPath(ctx context.Context, robot *model.Robot, finalGoal string, pathNodeIDs []string, walkBack bool) (*model.GlobalPath, error, string) {
	idInReject := func(ids []string, rejectIds map[string]string) bool {
		for _, cover := range ids {
			if _, find := rejectIds[cover]; find {
				return true
			}
		}
		return false
	}
	//查找对应路径，生成基本参数
	gp := planer.m.MapData.MakeGlobalPath(robot.Account.AgvId, robot.Status.MapName, robot.Status.Angle, finalGoal, pathNodeIDs)

	if walkBack {
		for _, globalPath := range gp.Paths {
			if globalPath.Dir == model.DirectionA2B {
				globalPath.BodyDir = model.HeadBA
			} else if globalPath.Dir == model.DirectionB2A {
				globalPath.BodyDir = model.HeadAB
			}
			globalPath.RotateType = 0
			globalPath.RotateEndAng = globalPath.RotateStartAng
			globalPath.PathEndAng = globalPath.RotateStartAng
		}
	}
	//终点锁计算
	target := gp.Paths.Back().GetTarget()
	//根据类型获得终点锁的计算参数
	//代价值计算
	lastPathTheta := angles.Deg2Rad(float64(robot.Status.Angle))
	for index, p := range gp.Paths {
		if index > 0 && p.RotateType != 0 {
			if planer.blockWithObstacle(p, float64(robot.Type.GetRobotRotateRadius(robot))) {
				p.RotateType *= -1
				if planer.blockWithObstacle(p, float64(robot.Type.GetRobotRotateRadius(robot))) {
					log.Warnf("robot:%s,两侧均有障碍物，无法旋转", robot.Account.AgvId)
					planer.heatingPathID(robot.Account.AgvId, "", p.PathId)
					return nil, codes.ErrPathPlanNoGlobalPath, ""
				}
			}
		}
		conflictWithLeft, conflictWithRight, blockAgv := false, false, ""
		if index < 3 && p.RotateType != 0 {
			allApprovedLocalPaths, errGetAll := planer.GetAllLocalPathFromDB(ctx)
			rejectIds, err2 := planer.GetPlanerAreaRejectNodes(ctx, robot, nil)
			if err2 != nil {
				return nil, err2, ""
			}
			if errGetAll == nil {
				for _, approved := range allApprovedLocalPaths {
					if approved.Sn != robot.Account.AgvId {
						//旋转判断车体扫过扇形区域与静止车辆的矩形区域是否相交
						if rotateBlock, _ := approved.RotateConflictRect(p, robot); rotateBlock {
							conflictWithLeft = true
							blockAgv = approved.Sn
							break
						}
					}
				}
			}

			if !conflictWithLeft {
				planer.UpdatePathLocks(robot.Account.AgvId, robot, p, lastPathTheta, robot.Type)
				if planer.blockWithObstacle(p, float64(robot.Type.GetRobotRotateRadius(robot))) ||
					idInReject(p.AllBlocks().ToIds(), rejectIds) {
					conflictWithLeft = true
				}
			}
			if conflictWithLeft {
				p.RotateType *= -1
				p.SourceBlocks = nil
				if errGetAll == nil {
					for _, approved := range allApprovedLocalPaths {
						if approved.Sn != robot.Account.AgvId {
							//旋转判断车体扫过扇形区域与静止车辆的矩形区域是否相交
							if rotateBlock, _ := approved.RotateConflictRect(p, robot); rotateBlock {
								conflictWithRight = true
								blockAgv = approved.Sn
								break
							}
						}
					}
				}
				if !conflictWithRight {
					planer.UpdatePathLocks(robot.Account.AgvId, robot, p, lastPathTheta, robot.Type)
					if planer.blockWithObstacle(p, robot.Type.GetRobotRotateRadius(robot)) ||
						idInReject(p.AllBlocks().ToIds(), rejectIds) {
						conflictWithRight = true
					}
				}
			}
			if conflictWithLeft && conflictWithRight {
				log.Warnf("robot:%s 两侧均有障碍物，无法旋转", robot.Account.AgvId)
				planer.heatingPathID(robot.Account.AgvId, "", p.PathId)
				return nil, codes.ErrCantRotate, blockAgv
			}
		}

		//判断当前头走，下一段尾走
		planer.UpdatePathLocks(robot.Account.AgvId, robot, p, lastPathTheta, robot.Type)
		// 判断上条路径是头走还是尾走
		isLastTailGo := false
		if index > 0 {
			lastP := gp.Paths[index-1]
			if lastP.BodyDirec == model.HeadBA && lastP.GetSource().Id == lastP.Path.NodeA.Id {
				isLastTailGo = true

			} else if lastP.BodyDirec == model.HeadAB && lastP.GetSource().Id == lastP.Path.NodeB.Id {
				isLastTailGo = true

			}

		}
		// log.Errorf("cost:-----start")
		cost := planer.getPathCostFunc(robot.Account.AgvId, robot, p, lastPathTheta, robot.Type, isLastTailGo)
		lastPathTheta = angles.Deg2Rad(float64(p.PathEndAng))
		gp.Cost += cost(robot.Account.AgvId)
		// log.Errorf("costEND:-----%f ,需要走的路径:%+v", gp.Cost, gp.Paths)
		//最后一个路径的点
		if index != len(gp.Paths)-1 {
			continue
		}
		x1, x2, y1, y2 := robot.Type.GotRobotPolygonRangeXY(robot, lastPathTheta, float64(p.GetTarget().X), float64(p.GetTarget().Y))
		robotRangeNodes := planer.NodeRangeSearch(x1, x2, y1, y2)
		for _, point := range robotRangeNodes {
			// p.GetTarget().Distance(point) < lockHeadRange &&
			if robot.Type.InLockRectangle(*point, robot, lastPathTheta, float64(p.GetTarget().X), float64(p.GetTarget().Y), 0, target.Type, true) { //计算锁定点角度判断
				gp.EndBlocks.Add(point.Id)
				gp.EndBlocks.Adds(point.AssociatedBlocks)
			}
		}
	}
	//更新路径末端锁
	gp.EndBlocks.Add(gp.Paths.Back().GetTarget().Id)
	for index, p := range gp.Paths {
		//除最后一个路径
		if index != len(gp.Paths)-1 {
			p.TargetBlocks = gp.Paths[index+1].SourceBlocks
			p.TargetRotate = gp.Paths[index+1].RotateType
			p.NextPathAng = gp.Paths[index+1].RotateEndAng
			continue
		}
		//最后一个路径
		p.TargetBlocks = gp.EndBlocks
	}
	//log.Debugf("全局路径代价:%f,锁定点代价:%f", gp.Cost, lockCost)
	return gp, nil, ""
}
func (planer *Planer) calculateGlobalPathCopy(ctx context.Context, robot *model.Robot, finalGoal string, pathNodeIDs []string, walkBack bool) (*model.GlobalPath, error) {
	//查找对应路径，生成基本参数
	gp := planer.m.MapData.MakeGlobalPath(robot.Account.AgvId, robot.Status.MapName, robot.Status.Angle, finalGoal, pathNodeIDs)

	if gp == nil || gp.Paths == nil {
		return nil, codes.ErrPathPlanNoGlobalPath
	}
	//终点锁计算
	target := gp.Paths.Back().GetTarget()

	//代价值计算
	lastPathTheta := angles.Deg2Rad(float64(robot.Status.Angle))
	for index, p := range gp.Paths {
		if index > 0 && p.RotateType != 0 {
			if planer.blockWithObstacle(p, float64(robot.Type.GetRobotRotateRadius(robot))) {
				p.RotateType *= -1
				if planer.blockWithObstacle(p, float64(robot.Type.GetRobotRotateRadius(robot))) {
					log.Warnf("robot:%s 两侧均有障碍物，无法旋转", robot.Account.AgvId)
					planer.heatingPathID(robot.Account.AgvId, "", p.PathId)
					return nil, codes.ErrPathPlanNoGlobalPath
				}
			}
		}
		conflictWithLeft, conflictWithRight := false, false
		if index == 0 && p.RotateType != 0 {
			if !conflictWithLeft {
				planer.UpdatePathLocks(robot.Account.AgvId, robot, p, lastPathTheta, robot.Type)
				if planer.blockWithObstacle(p, float64(robot.Type.GetRobotRotateRadius(robot))) {
					conflictWithLeft = true
				}
			}
			if conflictWithLeft {
				p.RotateType *= -1
				p.SourceBlocks = nil
				if !conflictWithRight {
					planer.UpdatePathLocks(robot.Account.AgvId, robot, p, lastPathTheta, robot.Type)
					if planer.blockWithObstacle(p, robot.Type.GetRobotRotateRadius(robot)) {
						conflictWithRight = true
					}
				}
			}
			if conflictWithLeft && conflictWithRight {
				// log.Warnf("robot:%s 两侧均有障碍物，无法旋转", robot.Account.AgvId)
				planer.heatingPathID(robot.Account.AgvId, "", p.PathId)
				return nil, codes.ErrCantRotate
			}
		}
		planer.UpdatePathLocks(robot.Account.AgvId, robot, p, lastPathTheta, robot.Type)
		// 判断上条路径是头走还是尾走
		isLastTailGo := false
		if index > 0 {
			lastP := gp.Paths[index-1]
			if lastP.BodyDirec == model.HeadBA && lastP.GetSource().Id == lastP.Path.NodeA.Id {
				isLastTailGo = true

			} else if lastP.BodyDirec == model.HeadAB && lastP.GetSource().Id == lastP.Path.NodeB.Id {
				isLastTailGo = true

			}
		}
		cost := planer.getPathCostFunc(robot.Account.AgvId, robot, p, lastPathTheta, robot.Type, isLastTailGo)
		lastPathTheta = angles.Deg2Rad(float64(p.PathEndAng))
		gp.Cost += cost(robot.Account.AgvId)
		//最后一个路径的点
		if index != len(gp.Paths)-1 {
			continue
		}
		x1, x2, y1, y2 := robot.Type.GotRobotPolygonRangeXY(robot, lastPathTheta, float64(p.GetTarget().X), float64(p.GetTarget().Y))
		robotRangeNodes := planer.NodeRangeSearch(x1, x2, y1, y2)
		for _, point := range robotRangeNodes {
			// p.GetTarget().Distance(point) < lockHeadRange &&
			if robot.Type.InLockRectangle(*point, robot, lastPathTheta, float64(p.GetTarget().X), float64(p.GetTarget().Y), float64(p.GetTarget().Type), target.Type, true) { //计算锁定点角度判断
				gp.EndBlocks.Add(point.Id)
				gp.EndBlocks.Adds(point.AssociatedBlocks)
			}
		}
	}
	//log.Debugf("全局路径代价:%f,锁定点代价:%f", gp.Cost, lockCost)
	return gp, nil
}

// 获取点半径范围内覆盖的点
func (planer *Planer) GetCircleRangeNode(node model.Node, r float64) []*model.Node {
	minX := float64(node.X) - r
	maxX := float64(node.X) + r
	minY := float64(node.Y) - r
	maxY := float64(node.Y) + r

	nodes := planer.NodeRangeSearch(minX, maxX, minY, maxY)
	return nodes
}

//获取路径代价值和锁点信息
func (planer *Planer) getPathCostFunc(agvID string, robot *model.Robot, p *model.TrafficPath, sourceBodyDirRad float64, robotType model.RobotType, optionalParam ...bool) func(sn string) float64 {
	// getPathCost := func(speed float64, maxSpeed float64, pathLength float64) float64 {
	// 	use_speed := 0.0
	// 	if speed <= 0 {
	// 		use_speed = maxSpeed
	// 	} else {
	// 		use_speed = math.Min(speed, maxSpeed)
	// 	}
	// 	//带价值
	// 	return pathLength / use_speed

	// }
	var isLastTailGo bool = false
	if len(optionalParam) > 0 {
		isLastTailGo = optionalParam[0]
	}
	cost := 0.0
	rotate, _ := calculateRotate(p.Path, sourceBodyDirRad) //起点旋转判定
	if p.Type == model.PathTypeBezier {
		rotate, _ = calculateBezierRotate(p.Path, sourceBodyDirRad) //起点旋转判定
	}
	rotateAbs := math.Abs(angles.Rad2Deg(rotate))

	//其余非拐点锁定
	speedLine := float64(robotType.MaxVel)
	SpeedCurve := float64(robotType.MaxAng)
	speedW := 90.0 / (float64(robotType.RotateTime) / 1000.0) //度/s
	//停车耗时
	stopTime := float64(robotType.StopTime)

	//判断当前路径是正着走还倒着走
	isTailGo := false
	if !isLastTailGo && rotateAbs < 10 {
		if p.BodyDirec == model.HeadBA && p.GetSource().Id == p.Path.NodeA.Id {
			isTailGo = true

		} else if p.BodyDirec == model.HeadAB && p.GetSource().Id == p.Path.NodeB.Id {
			isTailGo = true

		}
	}
	//判断是否是拐点
	if isTailGo {
		cost += stopTime / 1000
		// log.Errorf("当前路径:%d ,是否需要倒着走：%t 停车耗时:%f", p.Path.Id, isTailGo, stopTime/1000)
	}
	//直线代价
	//log.Debugf("l:%f / v:%f,cost:%f", p.Length(), float64(robot.Type.MaxVel), p.Length()/float64(robot.Type.MaxVel))
	//log.Infof("line cost:%f", p.Length()/speedLine)
	if p.Path.Type == model.PathTypeArc || p.Path.Type == model.PathTypeBezier {
		//
		// cost += getPathCost(p.PathVel, SpeedCurve, p.PathLength)
		cost += p.PathLength / SpeedCurve
	} else {
		// cost += getPathCost(p.PathVel, speedLine, p.PathLength)
		cost += p.PathLength / speedLine
	}

	//转弯代价
	//log.Debugf("r:%f / w:%f,cost:%f", rotateAbs, 90.0/float64(robot.Type.RotateTime), rotateAbs/90.0*float64(robot.Type.RotateTime)/1000)
	//log.Infof("w cost:%f", rotateAbs/speedW)
	cost += rotateAbs / speedW

	//判断当前路径车头走？

	//冲突代价计算
	return func(sn string) float64 {
		currentCost := planer.getConflictPathIDHeat(sn, p.PathId)
		if p.Path.GetTarget().Type == model.NodeStation || p.Path.GetTarget().Type == model.NodeCharge || p.Path.GetTarget().Type == model.NodeWaiting {
			// if p.Path.NodeA.Type != model.NodePath || p.Path.NodeB.Type != model.NodePath {
			currentCost += 20.0
		}
		return currentCost + cost
	}
}

//获取路径代价值和锁点信息
func (planer *Planer) UpdatePathLocks(agvID string, robot *model.Robot, p *model.TrafficPath, sourceBodyDirRad float64, robotType model.RobotType) bool {
	rotate, newBodyDir := calculateRotate(p.Path, sourceBodyDirRad) //起点旋转判定
	if p.Type == model.PathTypeBezier {
		rotate, newBodyDir = calculateBezierRotate(p.Path, sourceBodyDirRad) //起点旋转判定
	}
	rotateAbs := math.Abs(angles.Rad2Deg(rotate))
	isTurningPoint := false //是否为拐点
	//log.Debugf("拐点计算:(路径编号:%d,起点:%d,),路径车身方向类型:%d,车身初始角度：%f,旋转角度:%f", p.Id, p.GetSource().Id, p.BodyDir, bodyDirF, rotate)
	if rotateAbs > 10 {
		//log.Debugf("获得拐点:%d,旋转角度:%f", p.GetSource().Id, rotate)
		//旋转角度大于10度，认为是一个拐点
		isTurningPoint = true
	}
	//其余非拐点锁定
	robotR := robotType.GetRobotRotateRadius(robot)
	l0 := float64(p.GetSource().Distance(p.GetTarget()))                                             //路径长度（线段长度）
	x0, y0 := float64(p.GetSource().X+p.GetTarget().X)/2, float64(p.GetSource().Y+p.GetTarget().Y)/2 //路径中点
	var theta float64
	switch {
	case p.BodyDirec == model.HeadAB || p.BodyDirec == model.Head && p.Dir == model.DirectionA2B:
		theta = math.Atan2(float64(p.NodeB.Y-p.NodeA.Y), float64(p.NodeB.X-p.NodeA.X))
	case p.BodyDirec == model.HeadBA || p.BodyDirec == model.Head && p.Dir == model.DirectionB2A:
		theta = math.Atan2(float64(p.NodeA.Y-p.NodeB.Y), float64(p.NodeA.X-p.NodeB.X))
	default:
		panic("not here")
	}
	if p.Type == model.PathTypeBezier {
		p.PathPolygon = robotType.ComputeBezierPolygon(p, robot)
	} else {
		p.PathPolygon = robotType.GotLockRectangle(robot, theta, x0, y0, l0, 0, false, false)
	}
	if isTurningPoint {
		rangeNodes := planer.GetCircleRangeNode(p.GetSource(), robotR)
		inAngleCheck := inAngleRangeCheckWithRotateFunc(sourceBodyDirRad, newBodyDir, int(p.RotateType))
		for _, point := range rangeNodes {
			pointInSourceBlocks := false
			//路径起点锁，拐点：2倍车旋转半径
			dis := p.GetSource().Distance(*point)
			//角度判定,只锁定旋转范围内的点
			thetaOPRad := math.Atan2(float64(point.Y-p.GetSource().Y), float64(point.X-p.GetSource().X)) //判定点与路径起点连线在世界坐标系中的角度
			if inAngleCheck(thetaOPRad) && dis < robotR {
				p.SourceBlocks.Add(point.Id)
				pointInSourceBlocks = true
			} else if robotType.InLockRectangle(*point, robot, sourceBodyDirRad, float64(p.GetSource().X), float64(p.GetSource().Y), 0, 0, true) {
				p.SourceBlocks.Add(point.Id)
				pointInSourceBlocks = true
			} else if robotType.InLockRectangle(*point, robot, newBodyDir, float64(p.GetSource().X), float64(p.GetSource().Y), 0, 0, true) {
				p.SourceBlocks.Add(point.Id)
				pointInSourceBlocks = true
			}

			if pointInSourceBlocks {
				p.SourceBlocks.Adds(point.AssociatedBlocks)
			}
		}
	}

	x1, x2, y1, y2 := p.GetPolygonRangeXY()
	pathRangeNodes := planer.NodeRangeSearch(x1, x2, y1, y2)
	var rangeNodeIDs []string
	for _, n := range pathRangeNodes {
		rangeNodeIDs = append(rangeNodeIDs, n.Id)
	}
	//log.Warnf("range nodeIDs: %+v with params x1:%f x2:%f, y1:%f, y2:%f", rangeNodeIDs, x1, x2, y1, y2)
	for _, pathNode := range pathRangeNodes {
		//非拐点
		if p.PathPolygon.PointInPolygon(float64(pathNode.X), float64(pathNode.Y)) {
			//log.Warnf("path block%+v", lock.Id)
			p.PathBlocks.Add(pathNode.Id)
			p.PathBlocks.Adds(pathNode.AssociatedBlocks)
		}
	}
	return isTurningPoint
}
func (planer *Planer) GetRobotGlobalPaths(ctx context.Context, sn string) (*model.GlobalPath, codes.Code) {
	dbRes := planer.db.Collection(ctx, model.GlobalPath{}.TableName()).FindOne(ctx, bson.M{"sn": sn})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "get robot global path error:%s", dbRes.Err().Error())
		return nil, codes.ErrDatabase.Wrap(dbRes.Err())
	}
	if dbRes.Err() != nil && errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		return nil, codes.ErrGlobalPathNotFound
	}
	res := new(model.GlobalPath)
	if errBind := dbRes.Decode(res); errBind != nil {
		log.ErrorfWithContext(ctx, "bind robot global path error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return res, nil
}

func (planer *Planer) getAllGlobalPaths(ctx context.Context) ([]model.GlobalPath, codes.Code) {
	dbRes, err := planer.db.Collection(ctx, model.GlobalPath{}.TableName()).Find(ctx, bson.M{"map_name": planer.m.MapName})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all global path error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	res := make([]model.GlobalPath, 0)
	if errBind := dbRes.All(ctx, &res); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all global path error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return res, nil
}

//calculateRotate 单位：rad,计算以bodyTheta为初始角度，在该路径起点处需要旋转的角度
func calculateRotate(p model.Path, bodyTheta float64) (rotate, newBodyTheta float64) {
	thetaRad := p.BodyThetaRad()
	rotate = angles.ShortestAngularDistance(bodyTheta, thetaRad)
	newBodyTheta = angles.NormalizeAngle(thetaRad)
	return
}
func calculateBezierRotate(p model.Path, bodyTheta float64) (rotate, newBodyTheta float64) {
	thetaRad, _ := p.BodyBezierThetaRad()
	rotate = angles.ShortestAngularDistance(bodyTheta, thetaRad)
	newBodyTheta = angles.NormalizeAngle(thetaRad)
	return
}

//inAngleRangeCheckWithRotateFunc 加入路径起点的旋转类型，判断targetTheta是否在startTheta，endTheta范围内,输入均为弧度
func inAngleRangeCheckWithRotateFunc(startTheta, endTheta float64, rotate_type int) func(targetTheta float64) bool {
	//旋转角度小于160度的情况
	startTheta = angles.NormalizeAnglePositive(startTheta) //to [0,2pi]
	endTheta = angles.NormalizeAnglePositive(endTheta)     //to [0,2pi]
	//rotate_type 0=不旋转、1=逆时针旋转、-1=顺时针旋转
	return func(targetTheta float64) bool {
		targetTheta = angles.NormalizeAnglePositive(targetTheta) //to [0,2pi]
		if startTheta > endTheta {
			if targetTheta < startTheta && targetTheta > endTheta {
				if rotate_type == -1 {
					return true
				}
			} else {
				if rotate_type == 1 {
					return true
				}
			}
		} else {
			if targetTheta < endTheta && targetTheta > startTheta {
				if rotate_type == 1 {
					return true
				}
			} else {
				if rotate_type == -1 {
					return true
				}
			}
		}
		return false
	}
}
